10f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/* 20f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Copyright (C) 2011 The Android Open Source Project 30f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 40f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Licensed under the Apache License, Version 2.0 (the "License"); 50f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * you may not use this file except in compliance with the License. 60f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * You may obtain a copy of the License at 70f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 80f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * http://www.apache.org/licenses/LICENSE-2.0 90f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Unless required by applicable law or agreed to in writing, software 110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * distributed under the License is distributed on an "AS IS" BASIS, 120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * See the License for the specific language governing permissions and 140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * limitations under the License. 150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "rsMatrix2x2.h" 180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "rsMatrix3x3.h" 190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "rsMatrix4x4.h" 200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "stdlib.h" 220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "string.h" 230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#include "math.h" 240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesusing namespace android; 260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesusing namespace android::renderscript; 270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines////////////////////////////////////////////////////////////////////////////// 290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines// Heavy math functions 300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines////////////////////////////////////////////////////////////////////////////// 310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines// Returns true if the matrix was successfully inversed 370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesbool Matrix4x4::inverse() { 380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rs_matrix4x4 result; 390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int i, j; 410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (i = 0; i < 4; ++i) { 420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (j = 0; j < 4; ++j) { 430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines // computeCofactor for int i, int j 440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c0 = (i+1) % 4; 450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c1 = (i+2) % 4; 460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c2 = (i+3) % 4; 470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r0 = (j+1) % 4; 480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r1 = (j+2) % 4; 490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r2 = (j+3) % 4; 500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float minor = 520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1])) 530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0])) 540f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0])); 550f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 560f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float cofactor = (i+j) & 1 ? -minor : minor; 570f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 580f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines result.m[4*i + j] = cofactor; 590f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 600f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 610f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 620f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines // Dot product of 0th column of source and 0th row of result 630f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float det = m[0]*result.m[0] + m[4]*result.m[1] + 640f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8]*result.m[2] + m[12]*result.m[3]; 650f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 660f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (fabs(det) < 1e-6) { 670f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return false; 680f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 690f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 700f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines det = 1.0f / det; 710f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (i = 0; i < 16; ++i) { 720f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[i] = result.m[i] * det; 730f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 740f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 750f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return true; 760f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 770f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 780f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines// Returns true if the matrix was successfully inversed 790f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesbool Matrix4x4::inverseTranspose() { 800f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rs_matrix4x4 result; 810f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 820f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int i, j; 830f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (i = 0; i < 4; ++i) { 840f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (j = 0; j < 4; ++j) { 850f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines // computeCofactor for int i, int j 860f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c0 = (i+1) % 4; 870f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c1 = (i+2) % 4; 880f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int c2 = (i+3) % 4; 890f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r0 = (j+1) % 4; 900f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r1 = (j+2) % 4; 910f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int r2 = (j+3) % 4; 920f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 930f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float minor = (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1])) 940f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0])) 950f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0])); 960f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 970f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float cofactor = (i+j) & 1 ? -minor : minor; 980f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 990f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines result.m[4*j + i] = cofactor; 1000f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1010f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1020f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1030f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines // Dot product of 0th column of source and 0th column of result 1040f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float det = m[0]*result.m[0] + m[4]*result.m[4] + 1050f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8]*result.m[8] + m[12]*result.m[12]; 1060f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1070f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (fabs(det) < 1e-6) { 1080f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return false; 1090f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines det = 1.0f / det; 1120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (i = 0; i < 16; ++i) { 1130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[i] = result.m[i] * det; 1140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return true; 1170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::transpose() { 1200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines int i, j; 1210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float temp; 1220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (i = 0; i < 3; ++i) { 1230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (j = i + 1; j < 4; ++j) { 1240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines temp = m[i*4 + j]; 1250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[i*4 + j] = m[j*4 + i]; 1260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[j*4 + i] = temp; 1270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/////////////////////////////////////////////////////////////////////////////////// 1330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadIdentity() { 1350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[0] = 1.f; 1360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[1] = 0.f; 1370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[2] = 0.f; 1380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[3] = 0.f; 1390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[4] = 0.f; 1400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[5] = 1.f; 1410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[6] = 0.f; 1420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[7] = 0.f; 1430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8] = 0.f; 1440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[9] = 0.f; 1450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10] = 1.f; 1460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[11] = 0.f; 1470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12] = 0.f; 1480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13] = 0.f; 1490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14] = 0.f; 1500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[15] = 1.f; 1510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::load(const float *v) { 1540f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines memcpy(m, v, sizeof(m)); 1550f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1560f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1570f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::load(const rs_matrix4x4 *v) { 1580f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines memcpy(m, v->m, sizeof(m)); 1590f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1600f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1610f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::load(const rs_matrix3x3 *v) { 1620f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[0] = v->m[0]; 1630f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[1] = v->m[1]; 1640f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[2] = v->m[2]; 1650f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[3] = 0.f; 1660f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[4] = v->m[3]; 1670f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[5] = v->m[4]; 1680f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[6] = v->m[5]; 1690f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[7] = 0.f; 1700f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8] = v->m[6]; 1710f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[9] = v->m[7]; 1720f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10] = v->m[8]; 1730f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[11] = 0.f; 1740f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12] = 0.f; 1750f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13] = 0.f; 1760f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14] = 0.f; 1770f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[15] = 1.f; 1780f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1790f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1800f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::load(const rs_matrix2x2 *v) { 1810f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[0] = v->m[0]; 1820f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[1] = v->m[1]; 1830f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[2] = 0.f; 1840f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[3] = 0.f; 1850f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[4] = v->m[2]; 1860f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[5] = v->m[3]; 1870f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[6] = 0.f; 1880f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[7] = 0.f; 1890f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8] = 0.f; 1900f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[9] = 0.f; 1910f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10] = 1.f; 1920f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[11] = 0.f; 1930f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12] = 0.f; 1940f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13] = 0.f; 1950f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14] = 0.f; 1960f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[15] = 1.f; 1970f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1980f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1990f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2000f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadRotate(float rot, float x, float y, float z) { 2010f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float c, s; 2020f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[3] = 0; 2030f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[7] = 0; 2040f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[11]= 0; 2050f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12]= 0; 2060f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13]= 0; 2070f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14]= 0; 2080f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[15]= 1; 2090f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rot *= float(M_PI / 180.0f); 2100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines c = cosf(rot); 2110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines s = sinf(rot); 2120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float len = x*x + y*y + z*z; 2140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (len != 1) { 2150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float recipLen = 1.f / sqrtf(len); 2160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines x *= recipLen; 2170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines y *= recipLen; 2180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines z *= recipLen; 2190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float nc = 1.0f - c; 2210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float xy = x * y; 2220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float yz = y * z; 2230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float zx = z * x; 2240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float xs = x * s; 2250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float ys = y * s; 2260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float zs = z * s; 2270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 0] = x*x*nc + c; 2280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 4] = xy*nc - zs; 2290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 8] = zx*nc + ys; 2300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 1] = xy*nc + zs; 2310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 5] = y*y*nc + c; 2320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 9] = yz*nc - xs; 2330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 2] = zx*nc - ys; 2340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[ 6] = yz*nc + xs; 2350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10] = z*z*nc + c; 2360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadScale(float x, float y, float z) { 2390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines loadIdentity(); 2400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(0, 0, x); 2410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(1, 1, y); 2420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(2, 2, z); 2430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadTranslate(float x, float y, float z) { 2460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines loadIdentity(); 2470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12] = x; 2480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13] = y; 2490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14] = z; 2500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadMultiply(const rs_matrix4x4 *lhs, const rs_matrix4x4 *rhs) { 2530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (int i=0 ; i<4 ; i++) { 2540f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float ri0 = 0; 2550f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float ri1 = 0; 2560f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float ri2 = 0; 2570f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float ri3 = 0; 2580f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines for (int j=0 ; j<4 ; j++) { 2590f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float rhs_ij = ((const Matrix4x4 *)rhs)->get(i,j); 2600f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ri0 += ((const Matrix4x4 *)lhs)->get(j,0) * rhs_ij; 2610f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ri1 += ((const Matrix4x4 *)lhs)->get(j,1) * rhs_ij; 2620f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ri2 += ((const Matrix4x4 *)lhs)->get(j,2) * rhs_ij; 2630f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ri3 += ((const Matrix4x4 *)lhs)->get(j,3) * rhs_ij; 2640f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2650f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(i,0, ri0); 2660f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(i,1, ri1); 2670f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(i,2, ri2); 2680f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines set(i,3, ri3); 2690f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2700f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2710f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2720f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadOrtho(float left, float right, float bottom, float top, float near, float far) { 2730f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines loadIdentity(); 2740f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[0] = 2.f / (right - left); 2750f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[5] = 2.f / (top - bottom); 2760f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10]= -2.f / (far - near); 2770f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[12]= -(right + left) / (right - left); 2780f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[13]= -(top + bottom) / (top - bottom); 2790f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14]= -(far + near) / (far - near); 2800f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2810f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2820f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadFrustum(float left, float right, float bottom, float top, float near, float far) { 2830f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines loadIdentity(); 2840f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[0] = 2.f * near / (right - left); 2850f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[5] = 2.f * near / (top - bottom); 2860f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[8] = (right + left) / (right - left); 2870f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[9] = (top + bottom) / (top - bottom); 2880f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[10]= -(far + near) / (far - near); 2890f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[11]= -1.f; 2900f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[14]= -2.f * far * near / (far - near); 2910f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m[15]= 0.f; 2920f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2930f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2940f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::loadPerspective(float fovy, float aspect, float near, float far) { 2950f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float top = near * tan((float) (fovy * M_PI / 360.0f)); 2960f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float bottom = -top; 2970f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float left = bottom * aspect; 2980f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float right = top * aspect; 2990f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines loadFrustum(left, right, bottom, top, near, far); 3000f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 3010f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 3020f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::vectorMultiply(float *out, const float *in) const { 3030f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines out[0] = (m[0] * in[0]) + (m[4] * in[1]) + (m[8] * in[2]) + m[12]; 3040f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines out[1] = (m[1] * in[0]) + (m[5] * in[1]) + (m[9] * in[2]) + m[13]; 3050f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines out[2] = (m[2] * in[0]) + (m[6] * in[1]) + (m[10] * in[2]) + m[14]; 3060f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines out[3] = (m[3] * in[0]) + (m[7] * in[1]) + (m[11] * in[2]) + m[15]; 3070f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 3080f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 3090f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesvoid Matrix4x4::logv(const char *s) const { 3100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ALOGV("%s {%f, %f, %f, %f", s, m[0], m[4], m[8], m[12]); 3110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ALOGV("%s %f, %f, %f, %f", s, m[1], m[5], m[9], m[13]); 3120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ALOGV("%s %f, %f, %f, %f", s, m[2], m[6], m[10], m[14]); 3130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines ALOGV("%s %f, %f, %f, %f}", s, m[3], m[7], m[11], m[15]); 3140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 315