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