polyadvent

A game engine from scratch in C
git clone git://jb55.com/polyadvent
Log | Files | Refs | README

mat4.c (10129B)


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