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 
Matrix4x4_Copy(Matrix4x4 dst,Matrix4x4 src)12 void 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 
Matrix4x4_Multiply(Matrix4x4 result,Matrix4x4 mat1,Matrix4x4 mat2)20 void 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 
Matrix4x4_LoadIdentity(Matrix4x4 mat)36 void 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 
Matrix4x4_Scale(Matrix4x4 mat,float sx,float sy,float sz)46 void 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 
Matrix4x4_Translate(Matrix4x4 mat,float tx,float ty,float tz)58 void 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 
normalize(float * ax,float * ay,float * az)68 static 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 
Matrix4x4_Rotate(Matrix4x4 mat,float angle,float ax,float ay,float az)81 void 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 
Matrix4x4_Frustum(Matrix4x4 mat,float left,float right,float bottom,float top,float near,float far)129 void 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 
Matrix4x4_Perspective(Matrix4x4 mat,float fovy,float aspect,float nearZ,float farZ)166 void 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 
Matrix4x4_Transform(Matrix4x4 mat,float * x,float * y,float * z)179 void 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