187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams/* 287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * Copyright (C) 2011 The Android Open Source Project 387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * 487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * Licensed under the Apache License, Version 2.0 (the "License"); 587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * you may not use this file except in compliance with the License. 687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * You may obtain a copy of the License at 787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * 887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * http://www.apache.org/licenses/LICENSE-2.0 987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * 1087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * Unless required by applicable law or agreed to in writing, software 1187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * distributed under the License is distributed on an "AS IS" BASIS, 1287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * See the License for the specific language governing permissions and 1487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams * limitations under the License. 1587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams */ 1687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 1787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "rsMatrix2x2.h" 1887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "rsMatrix3x3.h" 1987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "rsMatrix4x4.h" 2087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 2187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "stdlib.h" 2287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "string.h" 2387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams#include "math.h" 2487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 2587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsusing namespace android; 2687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsusing namespace android::renderscript; 2787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 2887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams////////////////////////////////////////////////////////////////////////////// 2987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams// Heavy math functions 3087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams////////////////////////////////////////////////////////////////////////////// 3187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 3287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 3387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 3487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 3587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 3687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams// Returns true if the matrix was successfully inversed 3787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsbool Matrix4x4::inverse() { 3887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams rs_matrix4x4 result; 3987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 4087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int i, j; 4187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (i = 0; i < 4; ++i) { 4287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (j = 0; j < 4; ++j) { 4387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams // computeCofactor for int i, int j 4487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c0 = (i+1) % 4; 4587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c1 = (i+2) % 4; 4687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c2 = (i+3) % 4; 4787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r0 = (j+1) % 4; 4887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r1 = (j+2) % 4; 4987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r2 = (j+3) % 4; 5087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 5187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float minor = 5287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1])) 5387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0])) 5487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0])); 5587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 5687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float cofactor = (i+j) & 1 ? -minor : minor; 5787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 5887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams result.m[4*i + j] = cofactor; 5987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 6087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 6187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 6287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams // Dot product of 0th column of source and 0th row of result 6387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float det = m[0]*result.m[0] + m[4]*result.m[1] + 6487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8]*result.m[2] + m[12]*result.m[3]; 6587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 6687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams if (fabs(det) < 1e-6) { 6787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams return false; 6887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 6987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 7087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams det = 1.0f / det; 7187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (i = 0; i < 16; ++i) { 7287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[i] = result.m[i] * det; 7387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 7487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 7587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams return true; 7687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 7787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 7887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams// Returns true if the matrix was successfully inversed 7987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsbool Matrix4x4::inverseTranspose() { 8087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams rs_matrix4x4 result; 8187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 8287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int i, j; 8387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (i = 0; i < 4; ++i) { 8487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (j = 0; j < 4; ++j) { 8587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams // computeCofactor for int i, int j 8687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c0 = (i+1) % 4; 8787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c1 = (i+2) % 4; 8887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int c2 = (i+3) % 4; 8987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r0 = (j+1) % 4; 9087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r1 = (j+2) % 4; 9187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int r2 = (j+3) % 4; 9287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 9387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float minor = (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1])) 9487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0])) 9587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0])); 9687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 9787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float cofactor = (i+j) & 1 ? -minor : minor; 9887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 9987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams result.m[4*j + i] = cofactor; 10087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 10187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 10287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 10387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams // Dot product of 0th column of source and 0th column of result 10487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float det = m[0]*result.m[0] + m[4]*result.m[4] + 10587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8]*result.m[8] + m[12]*result.m[12]; 10687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 10787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams if (fabs(det) < 1e-6) { 10887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams return false; 10987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 11087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 11187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams det = 1.0f / det; 11287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (i = 0; i < 16; ++i) { 11387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[i] = result.m[i] * det; 11487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 11587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 11687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams return true; 11787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 11887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 11987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::transpose() { 12087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams int i, j; 12187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float temp; 12287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (i = 0; i < 3; ++i) { 12387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (j = i + 1; j < 4; ++j) { 12487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams temp = m[i*4 + j]; 12587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[i*4 + j] = m[j*4 + i]; 12687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[j*4 + i] = temp; 12787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 12887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 12987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 13087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 13187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 13287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams/////////////////////////////////////////////////////////////////////////////////// 13387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 13487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadIdentity() { 13587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[0] = 1.f; 13687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[1] = 0.f; 13787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[2] = 0.f; 13887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[3] = 0.f; 13987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[4] = 0.f; 14087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[5] = 1.f; 14187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[6] = 0.f; 14287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[7] = 0.f; 14387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8] = 0.f; 14487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[9] = 0.f; 14587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10] = 1.f; 14687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[11] = 0.f; 14787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12] = 0.f; 14887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13] = 0.f; 14987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14] = 0.f; 15087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[15] = 1.f; 15187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 15287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 15387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::load(const float *v) { 15487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams memcpy(m, v, sizeof(m)); 15587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 15687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 15787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::load(const rs_matrix4x4 *v) { 15887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams memcpy(m, v->m, sizeof(m)); 15987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 16087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 16187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::load(const rs_matrix3x3 *v) { 16287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[0] = v->m[0]; 16387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[1] = v->m[1]; 16487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[2] = v->m[2]; 16587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[3] = 0.f; 16687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[4] = v->m[3]; 16787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[5] = v->m[4]; 16887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[6] = v->m[5]; 16987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[7] = 0.f; 17087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8] = v->m[6]; 17187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[9] = v->m[7]; 17287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10] = v->m[8]; 17387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[11] = 0.f; 17487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12] = 0.f; 17587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13] = 0.f; 17687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14] = 0.f; 17787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[15] = 1.f; 17887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 17987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 18087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::load(const rs_matrix2x2 *v) { 18187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[0] = v->m[0]; 18287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[1] = v->m[1]; 18387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[2] = 0.f; 18487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[3] = 0.f; 18587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[4] = v->m[2]; 18687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[5] = v->m[3]; 18787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[6] = 0.f; 18887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[7] = 0.f; 18987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8] = 0.f; 19087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[9] = 0.f; 19187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10] = 1.f; 19287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[11] = 0.f; 19387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12] = 0.f; 19487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13] = 0.f; 19587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14] = 0.f; 19687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[15] = 1.f; 19787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 19887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 19987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 20087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadRotate(float rot, float x, float y, float z) { 20187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float c, s; 20287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[3] = 0; 20387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[7] = 0; 20487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[11]= 0; 20587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12]= 0; 20687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13]= 0; 20787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14]= 0; 20887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[15]= 1; 20987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams rot *= float(M_PI / 180.0f); 21087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams c = cosf(rot); 21187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams s = sinf(rot); 21287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 21387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float len = x*x + y*y + z*z; 21487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams if (len != 1) { 21587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float recipLen = 1.f / sqrtf(len); 21687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams x *= recipLen; 21787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams y *= recipLen; 21887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams z *= recipLen; 21987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 22087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float nc = 1.0f - c; 22187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float xy = x * y; 22287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float yz = y * z; 22387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float zx = z * x; 22487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float xs = x * s; 22587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float ys = y * s; 22687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float zs = z * s; 22787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 0] = x*x*nc + c; 22887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 4] = xy*nc - zs; 22987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 8] = zx*nc + ys; 23087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 1] = xy*nc + zs; 23187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 5] = y*y*nc + c; 23287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 9] = yz*nc - xs; 23387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 2] = zx*nc - ys; 23487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[ 6] = yz*nc + xs; 23587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10] = z*z*nc + c; 23687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 23787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 23887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadScale(float x, float y, float z) { 23987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams loadIdentity(); 24087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(0, 0, x); 24187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(1, 1, y); 24287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(2, 2, z); 24387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 24487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 24587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadTranslate(float x, float y, float z) { 24687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams loadIdentity(); 24787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12] = x; 24887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13] = y; 24987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14] = z; 25087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 25187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 25287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadMultiply(const rs_matrix4x4 *lhs, const rs_matrix4x4 *rhs) { 25387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (int i=0 ; i<4 ; i++) { 25487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float ri0 = 0; 25587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float ri1 = 0; 25687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float ri2 = 0; 25787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float ri3 = 0; 25887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams for (int j=0 ; j<4 ; j++) { 25987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams const float rhs_ij = ((const Matrix4x4 *)rhs)->get(i,j); 26087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams ri0 += ((const Matrix4x4 *)lhs)->get(j,0) * rhs_ij; 26187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams ri1 += ((const Matrix4x4 *)lhs)->get(j,1) * rhs_ij; 26287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams ri2 += ((const Matrix4x4 *)lhs)->get(j,2) * rhs_ij; 26387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams ri3 += ((const Matrix4x4 *)lhs)->get(j,3) * rhs_ij; 26487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 26587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(i,0, ri0); 26687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(i,1, ri1); 26787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(i,2, ri2); 26887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams set(i,3, ri3); 26987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams } 27087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 27187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 27287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadOrtho(float left, float right, float bottom, float top, float near, float far) { 27387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams loadIdentity(); 27487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[0] = 2.f / (right - left); 27587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[5] = 2.f / (top - bottom); 27687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10]= -2.f / (far - near); 27787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[12]= -(right + left) / (right - left); 27887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[13]= -(top + bottom) / (top - bottom); 27987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14]= -(far + near) / (far - near); 28087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 28187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 28287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadFrustum(float left, float right, float bottom, float top, float near, float far) { 28387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams loadIdentity(); 28487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[0] = 2.f * near / (right - left); 28587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[5] = 2.f * near / (top - bottom); 28687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[8] = (right + left) / (right - left); 28787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[9] = (top + bottom) / (top - bottom); 28887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[10]= -(far + near) / (far - near); 28987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[11]= -1.f; 29087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[14]= -2.f * far * near / (far - near); 29187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams m[15]= 0.f; 29287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 29387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 29487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::loadPerspective(float fovy, float aspect, float near, float far) { 29587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float top = near * tan((float) (fovy * M_PI / 360.0f)); 29687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float bottom = -top; 29787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float left = bottom * aspect; 29887fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams float right = top * aspect; 29987fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams loadFrustum(left, right, bottom, top, near, far); 30087fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 30187fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams 30287fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Samsvoid Matrix4x4::vectorMultiply(float *out, const float *in) const { 30387fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams out[0] = (m[0] * in[0]) + (m[4] * in[1]) + (m[8] * in[2]) + m[12]; 30487fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams out[1] = (m[1] * in[0]) + (m[5] * in[1]) + (m[9] * in[2]) + m[13]; 30587fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams out[2] = (m[2] * in[0]) + (m[6] * in[1]) + (m[10] * in[2]) + m[14]; 30687fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams out[3] = (m[3] * in[0]) + (m[7] * in[1]) + (m[11] * in[2]) + m[15]; 30787fe59a2f4d4c74539bfa0bff5f9a7e320e99415Jason Sams} 308f47fb9b7ce529cee60ad211634bb27ed623f1098Jason Sams 309f47fb9b7ce529cee60ad211634bb27ed623f1098Jason Samsvoid Matrix4x4::logv(const char *s) const { 3106598201f1c4f409defac9a5af789fb53a7cc00f8Steve Block ALOGV("%s {%f, %f, %f, %f", s, m[0], m[4], m[8], m[12]); 3116598201f1c4f409defac9a5af789fb53a7cc00f8Steve Block ALOGV("%s %f, %f, %f, %f", s, m[1], m[5], m[9], m[13]); 3126598201f1c4f409defac9a5af789fb53a7cc00f8Steve Block ALOGV("%s %f, %f, %f, %f", s, m[2], m[6], m[10], m[14]); 3136598201f1c4f409defac9a5af789fb53a7cc00f8Steve Block ALOGV("%s %f, %f, %f, %f}", s, m[3], m[7], m[11], m[15]); 314f47fb9b7ce529cee60ad211634bb27ed623f1098Jason Sams} 315