git.mutantstargoat.com Git - csgray/blob - src/matrix.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <math.h>
   4
   5 #define M(r, c) ((r << 2) + c)
   6
   7 static const float idmat[] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
   8
   9 void mat4_identity(float *m)
  10 {
  11         memcpy(m, idmat, sizeof idmat);
  12 }
  13
  14 void mat4_copy(float *dest, float *src)
  15 {
  16         memcpy(dest, src, 16 * sizeof(float));
  17 }
  18
  19 void mat4_mul(float *dest, float *a, float *b)
  20 {
  21         int i, j;
  22         float tmp[16];
  23
  24         if(dest == a) {
  25                 memcpy(tmp, a, sizeof tmp);
  26                 a = tmp;
  27         } else if(dest == b) {
  28                 memcpy(tmp, b, sizeof tmp);
  29                 b = tmp;
  30         }
  31
  32         for(i=0; i<4; i++) {
  33                 for(j=0; j<4; j++) {
  34                         dest[M(i, j)] = a[M(i, 0)] * b[M(0, j)] + a[M(i, 1)] * b[M(1, j)] +
  35                                 a[M(i, 2)] * b[M(2, j)] + a[M(i, 3)] * b[M(3, j)];
  36                 }
  37         }
  38 }
  39
  40
  41 void mat4_xform3(float *vdest, float *m, float *v)
  42 {
  43         float x = m[0] + v[0] + m[4] * v[1] + m[8] * v[2] + m[12];
  44         float y = m[1] + v[0] + m[5] * v[1] + m[9] * v[2] + m[13];
  45         vdest[2] = m[2] + v[0] + m[6] * v[1] + m[10] * v[2] + m[14];
  46         vdest[0] = x;
  47         vdest[1] = y;
  48 }
  49
  50 void mat4_xform4(float *vdest, float *m, float *v)
  51 {
  52         float x = m[0] + v[0] + m[4] * v[1] + m[8] * v[2] + m[12] * v[3];
  53         float y = m[1] + v[0] + m[5] * v[1] + m[9] * v[2] + m[13] * v[3];
  54         float z = m[2] + v[0] + m[6] * v[1] + m[10] * v[2] + m[14] * v[3];
  55         vdest[3] = m[3] + v[0] + m[7] * v[1] + m[11] * v[2] + m[15] * v[3];
  56         vdest[0] = x;
  57         vdest[1] = y;
  58         vdest[2] = z;
  59 }
  60
  61
  62 float *mat4_row(float *m, int row)
  63 {
  64         return m + (row << 2);
  65 }
  66
  67 float mat4_elem(float *m, int row, int col)
  68 {
  69         return m[M(row, col)];
  70 }
  71
  72
  73 void mat4_upper3x3(float *m)
  74 {
  75         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
  76         m[15] = 1.0f;
  77 }
  78
  79 #define swap(a, b)      \
  80         do { \
  81                 float tmp = a; \
  82                 a = b; \
  83                 b = tmp; \
  84         } while(0)
  85
  86 void mat4_transpose(float *m)
  87 {
  88         swap(m[1], m[4]);
  89         swap(m[2], m[8]);
  90         swap(m[3], m[12]);
  91         swap(m[6], m[9]);
  92         swap(m[7], m[13]);
  93         swap(m[11], m[14]);
  94 }
  95
  96 float mat4_determinant(float *m)
  97 {
  98         float d11 =     (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
  99                                 (m[M(2, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) +
 100                                 (m[M(3, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)]));
 101
 102         float d12 =     (m[M(0, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
 103                                 (m[M(2, 1)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 104                                 (m[M(3, 1)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)]));
 105
 106         float d13 =     (m[M(0, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) -
 107                                 (m[M(1, 1)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 108                                 (m[M(3, 1)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 109
 110         float d14 =     (m[M(0, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)])) -
 111                                 (m[M(1, 1)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)])) +
 112                                 (m[M(2, 1)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 113
 114         return m[M(0, 0)] * d11 - m[M(1, 0)] * d12 + m[M(2, 0)] * d13 - m[M(3, 0)] * d14;
 115 }
 116
 117 void mat4_adjoint(float *res, float *m)
 118 {
 119         int i, j;
 120         float cof[16];
 121
 122         cof[M(0, 0)] =  (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
 123                                         (m[M(2, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) +
 124                                         (m[M(3, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)]));
 125         cof[M(0, 1)] =  (m[M(0, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
 126                                         (m[M(2, 1)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 127                                         (m[M(3, 1)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)]));
 128         cof[M(0, 2)] =  (m[M(0, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) -
 129                                         (m[M(1, 1)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 130                                         (m[M(3, 1)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 131         cof[M(0, 3)] =  (m[M(0, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)])) -
 132                                         (m[M(1, 1)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)])) +
 133                                         (m[M(2, 1)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 134
 135         cof[M(1, 0)] =  (m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
 136                                         (m[M(2, 0)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) +
 137                                         (m[M(3, 0)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)]));
 138         cof[M(1, 1)] =  (m[M(0, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 2)])) -
 139                                         (m[M(2, 0)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 140                                         (m[M(3, 0)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)]));
 141         cof[M(1, 2)] =  (m[M(0, 0)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 2)])) -
 142                                         (m[M(1, 0)] * (m[M(0, 2)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 2)])) +
 143                                         (m[M(3, 0)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 144         cof[M(1, 3)] =  (m[M(0, 0)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 2)])) -
 145                                         (m[M(1, 0)] * (m[M(0, 2)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 2)])) +
 146                                         (m[M(2, 0)] * (m[M(0, 2)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 2)]));
 147
 148         cof[M(2, 0)] =  (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 1)])) -
 149                                         (m[M(2, 0)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 1)])) +
 150                                         (m[M(3, 0)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 1)]));
 151         cof[M(2, 1)] =  (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(2, 3)] * m[M(3, 1)])) -
 152                                         (m[M(2, 0)] * (m[M(0, 1)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 1)])) +
 153                                         (m[M(3, 0)] * (m[M(0, 1)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 1)]));
 154         cof[M(2, 2)] =  (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(1, 3)] * m[M(3, 1)])) -
 155                                         (m[M(1, 0)] * (m[M(0, 1)] * m[M(3, 3)] - m[M(0, 3)] * m[M(3, 1)])) +
 156                                         (m[M(3, 0)] * (m[M(0, 1)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 1)]));
 157         cof[M(2, 3)] =  (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(1, 3)] * m[M(2, 1)])) -
 158                                         (m[M(1, 0)] * (m[M(0, 1)] * m[M(2, 3)] - m[M(0, 3)] * m[M(2, 1)])) +
 159                                         (m[M(2, 0)] * (m[M(0, 1)] * m[M(1, 3)] - m[M(0, 3)] * m[M(1, 1)]));
 160
 161         cof[M(3, 0)] =  (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(2, 2)] * m[M(3, 1)])) -
 162                                         (m[M(2, 0)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(1, 2)] * m[M(3, 1)])) +
 163                                         (m[M(3, 0)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(1, 2)] * m[M(2, 1)]));
 164         cof[M(3, 1)] =  (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(2, 2)] * m[M(3, 1)])) -
 165                                         (m[M(2, 0)] * (m[M(0, 1)] * m[M(3, 2)] - m[M(0, 2)] * m[M(3, 1)])) +
 166                                         (m[M(3, 0)] * (m[M(0, 1)] * m[M(2, 2)] - m[M(0, 2)] * m[M(2, 1)]));
 167         cof[M(3, 2)] =  (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(1, 2)] * m[M(3, 1)])) -
 168                                         (m[M(1, 0)] * (m[M(0, 1)] * m[M(3, 2)] - m[M(0, 2)] * m[M(3, 1)])) +
 169                                         (m[M(3, 0)] * (m[M(0, 1)] * m[M(1, 2)] - m[M(0, 2)] * m[M(1, 1)]));
 170         cof[M(3, 3)] =  (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(1, 2)] * m[M(2, 1)])) -
 171                                         (m[M(1, 0)] * (m[M(0, 1)] * m[M(2, 2)] - m[M(0, 2)] * m[M(2, 1)])) +
 172                                         (m[M(2, 0)] * (m[M(0, 1)] * m[M(1, 2)] - m[M(0, 2)] * m[M(1, 1)]));
 173
 174         for(i=0; i<4; i++) {
 175                 for(j=0; j<4; j++) {
 176                         float val = j % 2 ? -cof[M(j, i)] : cof[M(j, i)];
 177                         res[M(j, i)] = (i % 2) ? -val : val;
 178                 }
 179         }
 180 }
 181
 182 int mat4_inverse(float *m)
 183 {
 184         int i, j;
 185         float adj[16];
 186         float det;
 187
 188         mat4_adjoint(adj, m);
 189         det = mat4_determinant(m);
 190         if(det == 0.0f) {
 191                 return -1;
 192         }
 193
 194         for(i=0; i<4; i++) {
 195                 for(j=0; j<4; j++) {
 196                         m[M(j, i)] = adj[M(j, i)] / det;
 197                 }
 198         }
 199         return 0;
 200 }
 201
 202
 203 void mat4_translation(float *m, float x, float y, float z)
 204 {
 205         mat4_identity(m);
 206         m[12] = x;
 207         m[13] = y;
 208         m[14] = z;
 209 }
 210
 211 void mat4_rotation_x(float *m, float angle)
 212 {
 213         float sa, ca;
 214
 215         mat4_identity(m);
 216         sa = sin(angle);
 217         ca = cos(angle);
 218         m[5] = ca;
 219         m[6] = sa;
 220         m[9] = -sa;
 221         m[10] = ca;
 222 }
 223
 224 void mat4_rotation_y(float *m, float angle)
 225 {
 226         float sa, ca;
 227
 228         mat4_identity(m);
 229         sa = sin(angle);
 230         ca = cos(angle);
 231         m[0] = ca;
 232         m[2] = -sa;
 233         m[8] = sa;
 234         m[10] = ca;
 235 }
 236
 237 void mat4_rotation_z(float *m, float angle)
 238 {
 239         float sa, ca;
 240
 241         mat4_identity(m);
 242         sa = sin(angle);
 243         ca = cos(angle);
 244         m[0] = ca;
 245         m[1] = sa;
 246         m[4] = -sa;
 247         m[5] = ca;
 248 }
 249
 250 void mat4_rotation(float *m, float angle, float x, float y, float z)
 251 {
 252         float sa = sin(angle);
 253         float ca = cos(angle);
 254         float invca = 1.0f - ca;
 255         float xsq = x * x;
 256         float ysq = y * y;
 257         float zsq = z * z;
 258
 259         mat4_identity(m);
 260         m[0] = xsq + (1.0f - xsq) * ca;
 261         m[4] = x * y * invca - z * sa;
 262         m[8] = x * z * invca + y * sa;
 263
 264         m[1] = x * y * invca + z * sa;
 265         m[5] = ysq + (1.0f - ysq) * ca;
 266         m[9] = y * z * invca - x * sa;
 267
 268         m[2] = x * z * invca - y * sa;
 269         m[6] = y * z * invca + x * sa;
 270         m[10] = zsq + (1.0f - zsq) * ca;
 271 }
 272
 273 void mat4_scaling(float *m, float sx, float sy, float sz)
 274 {
 275         mat4_identity(m);
 276         m[0] = sx;
 277         m[5] = sy;
 278         m[10] = sz;
 279 }
 280
 281
 282 void mat4_translate(float *m, float x, float y, float z)
 283 {
 284         float tmp[16];
 285         mat4_translation(tmp, x, y, z);
 286         mat4_mul(m, m, tmp);
 287 }
 288
 289 void mat4_rotate_x(float *m, float angle)
 290 {
 291         float tmp[16];
 292         mat4_rotation_x(tmp, angle);
 293         mat4_mul(m, m, tmp);
 294 }
 295
 296 void mat4_rotate_y(float *m, float angle)
 297 {
 298         float tmp[16];
 299         mat4_rotation_y(tmp, angle);
 300         mat4_mul(m, m, tmp);
 301 }
 302
 303 void mat4_rotate_z(float *m, float angle)
 304 {
 305         float tmp[16];
 306         mat4_rotation_z(tmp, angle);
 307         mat4_mul(m, m, tmp);
 308 }
 309
 310 void mat4_rotate(float *m, float angle, float x, float y, float z)
 311 {
 312         float tmp[16];
 313         mat4_rotation(tmp, angle, x, y, z);
 314         mat4_mul(m, m, tmp);
 315 }
 316
 317 void mat4_scale(float *m, float sx, float sy, float sz)
 318 {
 319         float tmp[16];
 320         mat4_scaling(tmp, sx, sy, sz);
 321         mat4_mul(m, m, tmp);
 322 }
 323
 324
 325 void mat4_pre_translate(float *m, float x, float y, float z)
 326 {
 327         float tmp[16];
 328         mat4_translation(tmp, x, y, z);
 329         mat4_mul(m, tmp, m);
 330 }
 331
 332 void mat4_pre_rotate_x(float *m, float angle)
 333 {
 334         float tmp[16];
 335         mat4_rotation_x(tmp, angle);
 336         mat4_mul(m, m, tmp);
 337 }
 338
 339 void mat4_pre_rotate_y(float *m, float angle)
 340 {
 341         float tmp[16];
 342         mat4_rotation_y(tmp, angle);
 343         mat4_mul(m, m, tmp);
 344 }
 345
 346 void mat4_pre_rotate_z(float *m, float angle)
 347 {
 348         float tmp[16];
 349         mat4_rotation_z(tmp, angle);
 350         mat4_mul(m, m, tmp);
 351 }
 352
 353 void mat4_pre_rotate(float *m, float angle, float x, float y, float z)
 354 {
 355         float tmp[16];
 356         mat4_rotation(tmp, angle, x, y, z);
 357         mat4_mul(m, m, tmp);
 358 }
 359
 360 void mat4_pre_scale(float *m, float sx, float sy, float sz)
 361 {
 362         float tmp[16];
 363         mat4_scaling(tmp, sx, sy, sz);
 364         mat4_mul(m, m, tmp);
 365 }
 366
 367 static void normalize(float *v)
 368 {
 369         float len = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
 370         if(len != 0.0f) {
 371                 float s = 1.0f / len;
 372                 v[0] *= s;
 373                 v[1] *= s;
 374                 v[2] *= s;
 375         }
 376 }
 377
 378 static void cross(float *res, float *a, float *b)
 379 {
 380         res[0] = a[1] * b[2] - a[2] * b[1];
 381         res[1] = a[2] * b[0] - a[0] * b[2];
 382         res[2] = a[0] * b[1] - a[1] * b[0];
 383 }
 384
 385 void mat4_lookat(float *m, float x, float y, float z, float tx, float ty, float tz, float ux, float uy, float uz)
 386 {
 387         int i;
 388         float dir[3], right[3], up[3];
 389
 390         dir[0] = tx - x;
 391         dir[1] = ty - y;
 392         dir[2] = tz - z;
 393         normalize(dir);
 394
 395         up[0] = ux;
 396         up[1] = uy;
 397         up[2] = uz;
 398         cross(right, dir, up);
 399         normalize(right);
 400
 401         cross(up, right, dir);
 402         normalize(up);
 403
 404         mat4_identity(m);
 405
 406         for(i=0; i<3; i++) {
 407                 m[i] = right[i];
 408                 m[i + 4] = up[i];
 409                 m[i + 8] = -dir[i];
 410         }
 411
 412         mat4_translate(m, x, y, z);
 413 }
 414
 415 void mat4_inv_lookat(float *m, float x, float y, float z, float tx, float ty, float tz, float ux, float uy, float uz)
 416 {
 417         int i;
 418         float dir[3], right[3], up[3];
 419
 420         dir[0] = tx - x;
 421         dir[1] = ty - y;
 422         dir[2] = tz - z;
 423         normalize(dir);
 424
 425         up[0] = ux;
 426         up[1] = uy;
 427         up[2] = uz;
 428         cross(right, dir, up);
 429         normalize(right);
 430
 431         cross(up, right, dir);
 432         normalize(up);
 433
 434         mat4_identity(m);
 435
 436         for(i=0; i<3; i++) {
 437                 m[i << 2] = right[i];
 438                 m[(i << 2) + 1] = up[i];
 439                 m[(i << 2) + 2] = -dir[i];
 440         }
 441
 442         mat4_pre_translate(m, -x, -y, -z);
 443 }
 444
 445 void mat4_ortho(float *m, float left, float right, float bottom, float top, float znear, float zfar)
 446 {
 447         float dx = right - left;
 448         float dy = top - bottom;
 449         float dz = zfar - znear;
 450
 451         mat4_identity(m);
 452         m[0] = 2.0f / dx;
 453         m[5] = 2.0f / dy;
 454         m[10] = -2.0f / dz;
 455         m[12] = -(right + left) / dx;
 456         m[13] = -(top + bottom) / dy;
 457         m[14] = -(zfar + znear) / dz;
 458 }
 459
 460 void mat4_frustum(float *m, float left, float right, float bottom, float top, float znear, float zfar)
 461 {
 462         float dx = right - left;
 463         float dy = top - bottom;
 464         float dz = zfar - znear;
 465
 466         memset(m, 0, 16 * sizeof *m);
 467         m[0] = 2.0f * znear / dx;
 468         m[5] = 2.0f * znear / dy;
 469         m[8] = (right + left) / dx;
 470         m[9] = (top + bottom) / dy;
 471         m[10] = -(zfar + znear) / dz;
 472         m[14] = -2.0f * zfar * znear / dz;
 473         m[11] = -1.0f;
 474 }
 475
 476 void mat4_perspective(float *m, float fov, float aspect, float znear, float zfar)
 477 {
 478         float s = 1.0f / tan(fov / 2.0f);
 479         float range = znear - zfar;
 480
 481         memset(m, 0, 16 * sizeof *m);
 482         m[0] = s / aspect;
 483         m[5] = s;
 484         m[10] = (znear + zfar) / range;
 485         m[14] = 2.0f * znear * zfar / range;
 486         m[11] = -1.0f;
 487 }
 488
 489
 490 void mat4_mirror(float *m, float a, float b, float c, float d)
 491 {
 492         m[0] = 1.0f - 2.0f * a * a;
 493         m[5] = 1.0f - 2.0f * b * b;
 494         m[10] = 1.0f - 2.0f * c * c;
 495         m[15] = 1.0f;
 496
 497         m[1] = m[4] = -2.0f * a * b;
 498         m[2] = m[8] = -2.0f * a * c;
 499         m[6] = m[9] = -2.0f * b * c;
 500
 501         m[12] = -2.0f * a * d;
 502         m[13] = -2.0f * b * d;
 503         m[14] = -2.0f * c * d;
 504
 505         m[3] = m[7] = m[11] = 0.0f;
 506 }
 507
 508
 509 void mat4_print(float *m, FILE *fp)
 510 {
 511         int i;
 512
 513         for(i=0; i<4; i++) {
 514                 fprintf(fp, "[ %4.4g %4.4g %4.4g %4.4g ]\n", m[0], m[1], m[2], m[3]);
 515                 m += 4;
 516         }
 517         fputc('\n', fp);
 518 }
 519