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 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 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 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 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 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 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 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 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 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 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
