mat4.c (10025B)
1 2 #include "mat4.h" 3 #include <math.h> 4 #include <stdio.h> 5 #include <string.h> 6 #include "vec3.h" 7 8 #define PI 3.14159265f 9 10 mat4 *mat4_id(mat4 *dst) { 11 dst[0] = 1.; dst[1] = 0.; dst[2] = 0.; dst[3] = 0.; 12 dst[4] = 0.; dst[5] = 1.; dst[6] = 0.; dst[7] = 0.; 13 dst[8] = 0.; dst[9] = 0.; dst[10] = 1.; dst[11] = 0.; 14 dst[12] = 0.; dst[13] = 0.; dst[14] = 0.; dst[15] = 1.; 15 16 return dst; 17 } 18 19 mat4 *mat4_transpose(mat4 *src, mat4 *dest) { 20 // If we are transposing ourselves we can skip a few steps but have to cache some values 21 if(dest == NULL || src == dest) { 22 float a01 = src[1], a02 = src[2], a03 = src[3]; 23 float a12 = src[6], a13 = src[7]; 24 float a23 = src[11]; 25 26 src[1] = src[4]; 27 src[2] = src[8]; 28 src[3] = src[12]; 29 src[4] = a01; 30 src[6] = src[9]; 31 src[7] = src[13]; 32 src[8] = a02; 33 src[9] = a12; 34 src[11] = src[14]; 35 src[12] = a03; 36 src[13] = a13; 37 src[14] = a23; 38 return src; 39 } 40 41 dest[0] = src[0]; 42 dest[1] = src[4]; 43 dest[2] = src[8]; 44 dest[3] = src[12]; 45 dest[4] = src[1]; 46 dest[5] = src[5]; 47 dest[6] = src[9]; 48 dest[7] = src[13]; 49 dest[8] = src[2]; 50 dest[9] = src[6]; 51 dest[10] = src[10]; 52 dest[11] = src[14]; 53 dest[12] = src[3]; 54 dest[13] = src[7]; 55 dest[14] = src[11]; 56 dest[15] = src[15]; 57 return dest; 58 59 } 60 61 /* 62 * mat4.frustum 63 * Generates a frustum matrix with the given bounds 64 * 65 * Params: 66 * left, right - scalar, left and right bounds of the frustum 67 * bottom, top - scalar, bottom and top bounds of the frustum 68 * near, far - scalar, near and far bounds of the frustum 69 * dest - Optional, mat4 frustum matrix will be written into 70 * 71 * Returns: 72 * dest if specified, a new mat4 otherwise 73 */ 74 mat4 *mat4_frustum (float left, float right, float bottom, 75 float top, float near, float far, mat4 *dest) { 76 float rl = (right - left); 77 float tb = (top - bottom); 78 float fn = (far - near); 79 dest[0] = (near*2) / rl; 80 dest[1] = 0; 81 dest[2] = 0; 82 dest[3] = 0; 83 dest[4] = 0; 84 dest[5] = (near*2) / tb; 85 dest[6] = 0; 86 dest[7] = 0; 87 dest[8] = (right + left) / rl; 88 dest[9] = (top + bottom) / tb; 89 dest[10] = -(far + near) / fn; 90 dest[11] = -1; 91 dest[12] = 0; 92 dest[13] = 0; 93 dest[14] = -(far*near*2) / fn; 94 dest[15] = 0; 95 return dest; 96 } 97 98 /* 99 * mat4.translate 100 * Translates a matrix by the given vector 101 * 102 * Params: 103 * mat - mat4 to translate 104 * vec - vec3 specifying the translation 105 * dest - Optional, mat4 receiving operation result. If not specified result is written to mat 106 * 107 * Returns: 108 * dest if specified, mat otherwise 109 */ 110 mat4 *mat4_translate (mat4 *mat, float *v3, mat4 *dest) { 111 const float x = v3[0]; 112 const float y = v3[1]; 113 const float z = v3[2]; 114 115 if(!dest || mat == dest) { 116 mat[12] = mat[0]*x + mat[4]*y + mat[8]*z + mat[12]; 117 mat[13] = mat[1]*x + mat[5]*y + mat[9]*z + mat[13]; 118 mat[14] = mat[2]*x + mat[6]*y + mat[10]*z + mat[14]; 119 mat[15] = mat[3]*x + mat[7]*y + mat[11]*z + mat[15]; 120 return mat; 121 } 122 123 float a00 = mat[0], a01 = mat[1], a02 = mat[2], a03 = mat[3]; 124 float a10 = mat[4], a11 = mat[5], a12 = mat[6], a13 = mat[7]; 125 float a20 = mat[8], a21 = mat[9], a22 = mat[10], a23 = mat[11]; 126 127 dest[0] = a00; 128 dest[1] = a01; 129 dest[2] = a02; 130 dest[3] = a03; 131 dest[4] = a10; 132 dest[5] = a11; 133 dest[6] = a12; 134 dest[7] = a13; 135 dest[8] = a20; 136 dest[9] = a21; 137 dest[10] = a22; 138 dest[11] = a23; 139 140 dest[12] = a00*x + a10*y + a20*z + mat[12]; 141 dest[13] = a01*x + a11*y + a21*z + mat[13]; 142 dest[14] = a02*x + a12*y + a22*z + mat[14]; 143 dest[15] = a03*x + a13*y + a23*z + mat[15]; 144 return dest; 145 } 146 147 mat4 *mat4_perspective(float fov, float aspect, float near, 148 float far, mat4 *dest) 149 { 150 float top = near * tanf(fov*PI / 360.0f); 151 float right = top * aspect; 152 return mat4_frustum(-right, right, -top, top, near, far, dest); 153 } 154 155 mat4 *mat4_ortho(float left, float right, float bottom, float top, float near, 156 float far, mat4 *dest) 157 { 158 float rl = 1.0 / (right - left); 159 float tb = 1.0 / (top - bottom); 160 float fn = 1.0 / (far - near); 161 dest[0] = 2.0 * rl; 162 dest[1] = 0; 163 dest[2] = 0; 164 dest[3] = 0; 165 dest[4] = 0; 166 dest[5] = 2.0 * tb; 167 dest[6] = 0; 168 dest[7] = 0; 169 dest[8] = 0; 170 dest[9] = 0; 171 dest[10] = -2.0 * fn; 172 dest[11] = 0; 173 dest[12] = (left + right) * rl; 174 dest[13] = (top + bottom) * tb; 175 dest[14] = (far + near) * fn; 176 dest[15] = 1.0; 177 return dest; 178 } 179 180 181 mat4 *mat4_inverse(mat4 *src, mat4 *dest) { 182 if(dest == NULL) { dest = src; } 183 184 // Cache the srcrix values (makes for huge speed increases!) 185 float a00 = src[0], a01 = src[1], a02 = src[2], a03 = src[3]; 186 float a10 = src[4], a11 = src[5], a12 = src[6], a13 = src[7]; 187 float a20 = src[8], a21 = src[9], a22 = src[10], a23 = src[11]; 188 float a30 = src[12], a31 = src[13], a32 = src[14], a33 = src[15]; 189 190 float b00 = a00*a11 - a01*a10; 191 float b01 = a00*a12 - a02*a10; 192 float b02 = a00*a13 - a03*a10; 193 float b03 = a01*a12 - a02*a11; 194 float b04 = a01*a13 - a03*a11; 195 float b05 = a02*a13 - a03*a12; 196 float b06 = a20*a31 - a21*a30; 197 float b07 = a20*a32 - a22*a30; 198 float b08 = a20*a33 - a23*a30; 199 float b09 = a21*a32 - a22*a31; 200 float b10 = a21*a33 - a23*a31; 201 float b11 = a22*a33 - a23*a32; 202 203 // Calculate the determinant (inlined to avoid double-caching) 204 float invDet = 1/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06); 205 206 dest[0] = (a11*b11 - a12*b10 + a13*b09)*invDet; 207 dest[1] = (-a01*b11 + a02*b10 - a03*b09)*invDet; 208 dest[2] = (a31*b05 - a32*b04 + a33*b03)*invDet; 209 dest[3] = (-a21*b05 + a22*b04 - a23*b03)*invDet; 210 dest[4] = (-a10*b11 + a12*b08 - a13*b07)*invDet; 211 dest[5] = (a00*b11 - a02*b08 + a03*b07)*invDet; 212 dest[6] = (-a30*b05 + a32*b02 - a33*b01)*invDet; 213 dest[7] = (a20*b05 - a22*b02 + a23*b01)*invDet; 214 dest[8] = (a10*b10 - a11*b08 + a13*b06)*invDet; 215 dest[9] = (-a00*b10 + a01*b08 - a03*b06)*invDet; 216 dest[10] = (a30*b04 - a31*b02 + a33*b00)*invDet; 217 dest[11] = (-a20*b04 + a21*b02 - a23*b00)*invDet; 218 dest[12] = (-a10*b09 + a11*b07 - a12*b06)*invDet; 219 dest[13] = (a00*b09 - a01*b07 + a02*b06)*invDet; 220 dest[14] = (-a30*b03 + a31*b01 - a32*b00)*invDet; 221 dest[15] = (a20*b03 - a21*b01 + a22*b00)*invDet; 222 223 return dest; 224 } 225 226 mat4 *mat4_remove_translations(mat4 *dest) { 227 dest[12] = 0.0; 228 dest[13] = 0.0; 229 dest[14] = 0.0; 230 dest[15] = 1.0; 231 return dest; 232 } 233 234 mat4 *mat4_multiply(const mat4 *a, const mat4 *b, mat4 *dst) { 235 float a00 = a[0], a01 = a[1], a02 = a[2], a03 = a[3]; 236 float a10 = a[4], a11 = a[5], a12 = a[6], a13 = a[7]; 237 float a20 = a[8], a21 = a[9], a22 = a[10], a23 = a[11]; 238 float a30 = a[12], a31 = a[13], a32 = a[14], a33 = a[15]; 239 240 float b00 = b[0], b01 = b[1], b02 = b[2], b03 = b[3]; 241 float b10 = b[4], b11 = b[5], b12 = b[6], b13 = b[7]; 242 float b20 = b[8], b21 = b[9], b22 = b[10], b23 = b[11]; 243 float b30 = b[12], b31 = b[13], b32 = b[14], b33 = b[15]; 244 245 dst[0] = b00*a00 + b01*a10 + b02*a20 + b03*a30; 246 dst[1] = b00*a01 + b01*a11 + b02*a21 + b03*a31; 247 dst[2] = b00*a02 + b01*a12 + b02*a22 + b03*a32; 248 dst[3] = b00*a03 + b01*a13 + b02*a23 + b03*a33; 249 dst[4] = b10*a00 + b11*a10 + b12*a20 + b13*a30; 250 dst[5] = b10*a01 + b11*a11 + b12*a21 + b13*a31; 251 dst[6] = b10*a02 + b11*a12 + b12*a22 + b13*a32; 252 dst[7] = b10*a03 + b11*a13 + b12*a23 + b13*a33; 253 dst[8] = b20*a00 + b21*a10 + b22*a20 + b23*a30; 254 dst[9] = b20*a01 + b21*a11 + b22*a21 + b23*a31; 255 dst[10] = b20*a02 + b21*a12 + b22*a22 + b23*a32; 256 dst[11] = b20*a03 + b21*a13 + b22*a23 + b23*a33; 257 dst[12] = b30*a00 + b31*a10 + b32*a20 + b33*a30; 258 dst[13] = b30*a01 + b31*a11 + b32*a21 + b33*a31; 259 dst[14] = b30*a02 + b31*a12 + b32*a22 + b33*a32; 260 dst[15] = b30*a03 + b31*a13 + b32*a23 + b33*a33; 261 262 return dst; 263 } 264 265 // TODO: util me 266 int float_eq(float a, float b); 267 int float_eq(float a, float b) { 268 return fabsf(a - b) < 0.0001; 269 } 270 271 272 mat4 *mat4_rotate(const mat4 *mat, const float angle, 273 const float *axis, mat4 *dest) { 274 float x = axis[0], y = axis[1], z = axis[2]; 275 float len = (float)sqrt(x*x + y*y + z*z); 276 277 if (float_eq(len, 0.)) { return NULL; } 278 // TODO: float comparison tool 279 if (!float_eq(len, 1.)) { 280 len = 1 / len; 281 x *= len; 282 y *= len; 283 z *= len; 284 } 285 286 float s = (float)sin(angle); 287 float c = (float)cos(angle); 288 float t = 1-c; 289 290 // Cache the matrix values (makes for huge speed increases!) 291 float a00 = mat[0], a01 = mat[1], a02 = mat[2], a03 = mat[3]; 292 float a10 = mat[4], a11 = mat[5], a12 = mat[6], a13 = mat[7]; 293 float a20 = mat[8], a21 = mat[9], a22 = mat[10], a23 = mat[11]; 294 295 // Construct the elements of the rotation matrix 296 float b00 = x*x*t + c, b01 = y*x*t + z*s, b02 = z*x*t - y*s; 297 float b10 = x*y*t - z*s, b11 = y*y*t + c, b12 = z*y*t + x*s; 298 float b20 = x*z*t + y*s, b21 = y*z*t - x*s, b22 = z*z*t + c; 299 300 // If the source and destination differ, copy the unchanged last row 301 if(mat != dest) { 302 dest[12] = mat[12]; 303 dest[13] = mat[13]; 304 dest[14] = mat[14]; 305 dest[15] = mat[15]; 306 } 307 308 // Perform rotation-specific matrix multiplication 309 dest[0] = a00*b00 + a10*b01 + a20*b02; 310 dest[1] = a01*b00 + a11*b01 + a21*b02; 311 dest[2] = a02*b00 + a12*b01 + a22*b02; 312 dest[3] = a03*b00 + a13*b01 + a23*b02; 313 314 dest[4] = a00*b10 + a10*b11 + a20*b12; 315 dest[5] = a01*b10 + a11*b11 + a21*b12; 316 dest[6] = a02*b10 + a12*b11 + a22*b12; 317 dest[7] = a03*b10 + a13*b11 + a23*b12; 318 319 dest[8] = a00*b20 + a10*b21 + a20*b22; 320 dest[9] = a01*b20 + a11*b21 + a21*b22; 321 dest[10] = a02*b20 + a12*b21 + a22*b22; 322 dest[11] = a03*b20 + a13*b21 + a23*b22; 323 324 return dest; 325 } 326 327 mat4 *mat4_scale(mat4 *a, float v[3], mat4 *out) { 328 float x = v[0], y = v[1], z = v[2]; 329 330 out[0] = a[0] * x; 331 out[1] = a[1] * x; 332 out[2] = a[2] * x; 333 out[3] = a[3] * x; 334 out[4] = a[4] * y; 335 out[5] = a[5] * y; 336 out[6] = a[6] * y; 337 out[7] = a[7] * y; 338 out[8] = a[8] * z; 339 out[9] = a[9] * z; 340 out[10] = a[10] * z; 341 out[11] = a[11] * z; 342 out[12] = a[12]; 343 out[13] = a[13]; 344 out[14] = a[14]; 345 out[15] = a[15]; 346 return out; 347 }; 348 349 350 void mat4_print(const mat4 *m) { 351 for (int i = 0; i < 16; ++i) { 352 if (i % 4 == 0) 353 printf("\n"); 354 printf("%f, ", m[i]); 355 } 356 printf("\n"); 357 } 358