1// Copyright (c) 2010 The Chromium OS Authors. All rights reserved.
2// Use of this source code is governed by a BSD-style license that can be
3// found in the LICENSE file.
4
5#include "matrixop.h"
6
7#include <math.h>
8
9#define PI 3.1415926535897932384626433832795f
10
11
12void Matrix4x4_Copy(Matrix4x4 dst, Matrix4x4 src)
13{
14    int i;
15    for (i = 0; i < 16; ++i)
16        dst[i] = src[i];
17}
18
19
20void Matrix4x4_Multiply(Matrix4x4 result, Matrix4x4 mat1, Matrix4x4 mat2)
21{
22    Matrix4x4 tmp;
23    int i, j, k;
24    for (i = 0; i < 4; i++)
25    {
26        for (j = 0; j < 4; ++j) {
27            tmp[i*4 + j] = 0;
28            for (k = 0; k < 4; ++k)
29                tmp[i*4 + j] += mat1[i*4 + k] * mat2[k*4 + j];
30        }
31    }
32    Matrix4x4_Copy(result, tmp);
33}
34
35
36void Matrix4x4_LoadIdentity(Matrix4x4 mat)
37{
38    int i;
39    for (i = 0; i < 16; ++i)
40        mat[i] = 0;
41    for (i = 0; i < 4; ++i)
42        mat[i*4 + i] = 1.f;
43}
44
45
46void Matrix4x4_Scale(Matrix4x4 mat, float sx, float sy, float sz)
47{
48    int i;
49    for (i = 0; i < 4; ++i)
50    {
51        mat[0*4 + i] *= sx;
52        mat[1*4 + i] *= sy;
53        mat[2*4 + i] *= sz;
54    }
55}
56
57
58void Matrix4x4_Translate(Matrix4x4 mat, float tx, float ty, float tz)
59{
60    int i;
61    for (i = 0; i < 4; ++i)
62        mat[3*4 + i] += mat[0*4 + i] * tx +
63                        mat[1*4 + i] * ty +
64                        mat[2*4 + i] * tz;
65}
66
67
68static float normalize(float *ax, float *ay, float *az)
69{
70    float norm = sqrtf((*ax) * (*ax) + (*ay) * (*ay) + (*az) * (*az));
71    if (norm > 0)
72    {
73        *ax /= norm;
74        *ay /= norm;
75        *az /= norm;
76    }
77    return norm;
78}
79
80
81void Matrix4x4_Rotate(Matrix4x4 mat, float angle,
82                      float ax, float ay, float az)
83{
84    Matrix4x4 rot;
85    float r = angle * PI / 180.f;
86    float s = sinf(r);
87    float c = cosf(r);
88    float one_c = 1.f - c;
89    float xx, yy, zz, xy, yz, xz, xs, ys, zs;
90    float norm = normalize(&ax, &ay, &az);
91
92    if (norm == 0 || angle == 0)
93        return;
94
95    xx = ax * ax;
96    yy = ay * ay;
97    zz = az * az;
98    xy = ax * ay;
99    yz = ay * az;
100    xz = ax * az;
101    xs = ax * s;
102    ys = ay * s;
103    zs = az * s;
104
105    rot[0*4 + 0] = xx + (1.f - xx) * c;
106    rot[1*4 + 0] = xy * one_c - zs;
107    rot[2*4 + 0] = xz * one_c + ys;
108    rot[3*4 + 0] = 0.f;
109
110    rot[0*4 + 1] = xy * one_c + zs;
111    rot[1*4 + 1] = yy + (1.f - yy) * c;
112    rot[2*4 + 1] = yz * one_c - xs;
113    rot[3*4 + 1] = 0.f;
114
115    rot[0*4 + 2] = xz * one_c - ys;
116    rot[1*4 + 2] = yz * one_c + xs;
117    rot[2*4 + 2] = zz + (1.f - zz) * c;
118    rot[3*4 + 2] = 0.f;
119
120    rot[0*4 + 3] = 0.f;
121    rot[1*4 + 3] = 0.f;
122    rot[2*4 + 3] = 0.f;
123    rot[3*4 + 3] = 1.f;
124
125    Matrix4x4_Multiply(mat, rot, mat);
126}
127
128
129void Matrix4x4_Frustum(Matrix4x4 mat,
130                       float left, float right,
131                       float bottom, float top,
132                       float near, float far)
133{
134    float dx = right - left;
135    float dy = top - bottom;
136    float dz = far - near;
137    Matrix4x4 frust;
138
139    if (near <= 0.f || far <= 0.f || dx <= 0.f || dy <= 0.f || dz <= 0.f)
140        return;
141
142    frust[0*4 + 0] = 2.f * near / dx;
143    frust[0*4 + 1] = 0.f;
144    frust[0*4 + 2] = 0.f;
145    frust[0*4 + 3] = 0.f;
146
147    frust[1*4 + 0] = 0.f;
148    frust[1*4 + 1] = 2.f * near / dy;
149    frust[1*4 + 2] = 0.f;
150    frust[1*4 + 3] = 0.f;
151
152    frust[2*4 + 0] = (right + left) / dx;
153    frust[2*4 + 1] = (top + bottom) / dy;
154    frust[2*4 + 2] = -(near + far) / dz;
155    frust[2*4 + 3] = -1.f;
156
157    frust[3*4 + 0] = 0.f;
158    frust[3*4 + 1] = 0.f;
159    frust[3*4 + 2] = -2.f * near * far / dz;
160    frust[3*4 + 3] = 0.f;
161
162    Matrix4x4_Multiply(mat, frust, mat);
163}
164
165
166void Matrix4x4_Perspective(Matrix4x4 mat,
167                           float fovy, float aspect,
168                           float nearZ, float farZ)
169{
170    float frustumW, frustumH;
171    frustumH = tanf(fovy / 360.f * PI) * nearZ;
172    frustumW = frustumH * aspect;
173
174    Matrix4x4_Frustum(mat, -frustumW, frustumW,
175                      -frustumH, frustumH, nearZ, farZ);
176}
177
178
179void Matrix4x4_Transform(Matrix4x4 mat, float *x, float *y, float *z)
180{
181    float tx = mat[0*4 + 0] * (*x) + mat[1*4 + 0] * (*y) + mat[2*4 + 0] * (*z);
182    float ty = mat[0*4 + 1] * (*x) + mat[1*4 + 1] * (*y) + mat[2*4 + 1] * (*z);
183    float tz = mat[0*4 + 2] * (*x) + mat[1*4 + 2] * (*y) + mat[2*4 + 2] * (*z);
184    *x = tx;
185    *y = ty;
186    *z = tz;
187}
188
189