1/*
2 * Copyright (C) 2011 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
17#include "rsMatrix2x2.h"
18#include "rsMatrix3x3.h"
19#include "rsMatrix4x4.h"
20
21#include "stdlib.h"
22#include "string.h"
23#include "math.h"
24
25using namespace android;
26using namespace android::renderscript;
27
28//////////////////////////////////////////////////////////////////////////////
29// Heavy math functions
30//////////////////////////////////////////////////////////////////////////////
31
32
33
34
35
36// Returns true if the matrix was successfully inversed
37bool Matrix4x4::inverse() {
38    rs_matrix4x4 result;
39
40    int i, j;
41    for (i = 0; i < 4; ++i) {
42        for (j = 0; j < 4; ++j) {
43            // computeCofactor for int i, int j
44            int c0 = (i+1) % 4;
45            int c1 = (i+2) % 4;
46            int c2 = (i+3) % 4;
47            int r0 = (j+1) % 4;
48            int r1 = (j+2) % 4;
49            int r2 = (j+3) % 4;
50
51            float minor =
52                (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1]))
53                - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0]))
54                + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0]));
55
56            float cofactor = (i+j) & 1 ? -minor : minor;
57
58            result.m[4*i + j] = cofactor;
59        }
60    }
61
62    // Dot product of 0th column of source and 0th row of result
63    float det = m[0]*result.m[0] + m[4]*result.m[1] +
64                 m[8]*result.m[2] + m[12]*result.m[3];
65
66    if (fabs(det) < 1e-6) {
67        return false;
68    }
69
70    det = 1.0f / det;
71    for (i = 0; i < 16; ++i) {
72        m[i] = result.m[i] * det;
73    }
74
75    return true;
76}
77
78// Returns true if the matrix was successfully inversed
79bool Matrix4x4::inverseTranspose() {
80    rs_matrix4x4 result;
81
82    int i, j;
83    for (i = 0; i < 4; ++i) {
84        for (j = 0; j < 4; ++j) {
85            // computeCofactor for int i, int j
86            int c0 = (i+1) % 4;
87            int c1 = (i+2) % 4;
88            int c2 = (i+3) % 4;
89            int r0 = (j+1) % 4;
90            int r1 = (j+2) % 4;
91            int r2 = (j+3) % 4;
92
93            float minor = (m[c0 + 4*r0] * (m[c1 + 4*r1] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r1]))
94                         - (m[c0 + 4*r1] * (m[c1 + 4*r0] * m[c2 + 4*r2] - m[c1 + 4*r2] * m[c2 + 4*r0]))
95                         + (m[c0 + 4*r2] * (m[c1 + 4*r0] * m[c2 + 4*r1] - m[c1 + 4*r1] * m[c2 + 4*r0]));
96
97            float cofactor = (i+j) & 1 ? -minor : minor;
98
99            result.m[4*j + i] = cofactor;
100        }
101    }
102
103    // Dot product of 0th column of source and 0th column of result
104    float det = m[0]*result.m[0] + m[4]*result.m[4] +
105                 m[8]*result.m[8] + m[12]*result.m[12];
106
107    if (fabs(det) < 1e-6) {
108        return false;
109    }
110
111    det = 1.0f / det;
112    for (i = 0; i < 16; ++i) {
113        m[i] = result.m[i] * det;
114    }
115
116    return true;
117}
118
119void Matrix4x4::transpose() {
120    int i, j;
121    float temp;
122    for (i = 0; i < 3; ++i) {
123        for (j = i + 1; j < 4; ++j) {
124            temp = m[i*4 + j];
125            m[i*4 + j] = m[j*4 + i];
126            m[j*4 + i] = temp;
127        }
128    }
129}
130
131
132///////////////////////////////////////////////////////////////////////////////////
133
134void Matrix4x4::loadIdentity() {
135    m[0] = 1.f;
136    m[1] = 0.f;
137    m[2] = 0.f;
138    m[3] = 0.f;
139    m[4] = 0.f;
140    m[5] = 1.f;
141    m[6] = 0.f;
142    m[7] = 0.f;
143    m[8] = 0.f;
144    m[9] = 0.f;
145    m[10] = 1.f;
146    m[11] = 0.f;
147    m[12] = 0.f;
148    m[13] = 0.f;
149    m[14] = 0.f;
150    m[15] = 1.f;
151}
152
153void Matrix4x4::load(const float *v) {
154    memcpy(m, v, sizeof(m));
155}
156
157void Matrix4x4::load(const rs_matrix4x4 *v) {
158    memcpy(m, v->m, sizeof(m));
159}
160
161void Matrix4x4::load(const rs_matrix3x3 *v) {
162    m[0] = v->m[0];
163    m[1] = v->m[1];
164    m[2] = v->m[2];
165    m[3] = 0.f;
166    m[4] = v->m[3];
167    m[5] = v->m[4];
168    m[6] = v->m[5];
169    m[7] = 0.f;
170    m[8] = v->m[6];
171    m[9] = v->m[7];
172    m[10] = v->m[8];
173    m[11] = 0.f;
174    m[12] = 0.f;
175    m[13] = 0.f;
176    m[14] = 0.f;
177    m[15] = 1.f;
178}
179
180void Matrix4x4::load(const rs_matrix2x2 *v) {
181    m[0] = v->m[0];
182    m[1] = v->m[1];
183    m[2] = 0.f;
184    m[3] = 0.f;
185    m[4] = v->m[2];
186    m[5] = v->m[3];
187    m[6] = 0.f;
188    m[7] = 0.f;
189    m[8] = 0.f;
190    m[9] = 0.f;
191    m[10] = 1.f;
192    m[11] = 0.f;
193    m[12] = 0.f;
194    m[13] = 0.f;
195    m[14] = 0.f;
196    m[15] = 1.f;
197}
198
199
200void Matrix4x4::loadRotate(float rot, float x, float y, float z) {
201    float c, s;
202    m[3] = 0;
203    m[7] = 0;
204    m[11]= 0;
205    m[12]= 0;
206    m[13]= 0;
207    m[14]= 0;
208    m[15]= 1;
209    rot *= float(M_PI / 180.0f);
210    c = cosf(rot);
211    s = sinf(rot);
212
213    const float len = x*x + y*y + z*z;
214    if (len != 1) {
215        const float recipLen = 1.f / sqrtf(len);
216        x *= recipLen;
217        y *= recipLen;
218        z *= recipLen;
219    }
220    const float nc = 1.0f - c;
221    const float xy = x * y;
222    const float yz = y * z;
223    const float zx = z * x;
224    const float xs = x * s;
225    const float ys = y * s;
226    const float zs = z * s;
227    m[ 0] = x*x*nc +  c;
228    m[ 4] =  xy*nc - zs;
229    m[ 8] =  zx*nc + ys;
230    m[ 1] =  xy*nc + zs;
231    m[ 5] = y*y*nc +  c;
232    m[ 9] =  yz*nc - xs;
233    m[ 2] =  zx*nc - ys;
234    m[ 6] =  yz*nc + xs;
235    m[10] = z*z*nc +  c;
236}
237
238void Matrix4x4::loadScale(float x, float y, float z) {
239    loadIdentity();
240    set(0, 0, x);
241    set(1, 1, y);
242    set(2, 2, z);
243}
244
245void Matrix4x4::loadTranslate(float x, float y, float z) {
246    loadIdentity();
247    m[12] = x;
248    m[13] = y;
249    m[14] = z;
250}
251
252void Matrix4x4::loadMultiply(const rs_matrix4x4 *lhs, const rs_matrix4x4 *rhs) {
253    // Use a temporary variable to support the case where one of the inputs
254    // is also the destination, e.g. left.loadMultiply(left, right);
255    Matrix4x4 temp;
256    for (int i=0 ; i<4 ; i++) {
257        float ri0 = 0;
258        float ri1 = 0;
259        float ri2 = 0;
260        float ri3 = 0;
261        for (int j=0 ; j<4 ; j++) {
262            const float rhs_ij = ((const Matrix4x4 *)rhs)->get(i,j);
263            ri0 += ((const Matrix4x4 *)lhs)->get(j,0) * rhs_ij;
264            ri1 += ((const Matrix4x4 *)lhs)->get(j,1) * rhs_ij;
265            ri2 += ((const Matrix4x4 *)lhs)->get(j,2) * rhs_ij;
266            ri3 += ((const Matrix4x4 *)lhs)->get(j,3) * rhs_ij;
267        }
268        temp.set(i,0, ri0);
269        temp.set(i,1, ri1);
270        temp.set(i,2, ri2);
271        temp.set(i,3, ri3);
272    }
273    load(&temp);
274}
275
276void Matrix4x4::loadOrtho(float left, float right, float bottom, float top, float near, float far) {
277    loadIdentity();
278    m[0] = 2.f / (right - left);
279    m[5] = 2.f / (top - bottom);
280    m[10]= -2.f / (far - near);
281    m[12]= -(right + left) / (right - left);
282    m[13]= -(top + bottom) / (top - bottom);
283    m[14]= -(far + near) / (far - near);
284}
285
286void Matrix4x4::loadFrustum(float left, float right, float bottom, float top, float near, float far) {
287    loadIdentity();
288    m[0] = 2.f * near / (right - left);
289    m[5] = 2.f * near / (top - bottom);
290    m[8] = (right + left) / (right - left);
291    m[9] = (top + bottom) / (top - bottom);
292    m[10]= -(far + near) / (far - near);
293    m[11]= -1.f;
294    m[14]= -2.f * far * near / (far - near);
295    m[15]= 0.f;
296}
297
298void Matrix4x4::loadPerspective(float fovy, float aspect, float near, float far) {
299    float top = near * tan((float) (fovy * M_PI / 360.0f));
300    float bottom = -top;
301    float left = bottom * aspect;
302    float right = top * aspect;
303    loadFrustum(left, right, bottom, top, near, far);
304}
305
306// Note: This assumes that the input vector (in) is of length 3.
307void Matrix4x4::vectorMultiply(float *out, const float *in) const {
308    out[0] = (m[0] * in[0]) + (m[4] * in[1]) + (m[8] * in[2]) + m[12];
309    out[1] = (m[1] * in[0]) + (m[5] * in[1]) + (m[9] * in[2]) + m[13];
310    out[2] = (m[2] * in[0]) + (m[6] * in[1]) + (m[10] * in[2]) + m[14];
311    out[3] = (m[3] * in[0]) + (m[7] * in[1]) + (m[11] * in[2]) + m[15];
312}
313
314void Matrix4x4::logv(const char *s) const {
315    ALOGV("%s {%f, %f, %f, %f",  s, m[0], m[4], m[8], m[12]);
316    ALOGV("%s  %f, %f, %f, %f",  s, m[1], m[5], m[9], m[13]);
317    ALOGV("%s  %f, %f, %f, %f",  s, m[2], m[6], m[10], m[14]);
318    ALOGV("%s  %f, %f, %f, %f}", s, m[3], m[7], m[11], m[15]);
319}
320