git.rot13.org Git - simavr/blob - examples/shared/libc3/src/c3algebra.c

   1 /*
   2         c3algebra.c
   3
   4         Copyright 2008-2012 Michel Pollet <buserror@gmail.com>
   5
   6         Derivative and inspiration from original C++:
   7         Paul Rademacher & Jean-Francois DOUEG,
   8
   9         This file is part of libc3.
  10
  11         libc3 is free software: you can redistribute it and/or modify
  12         it under the terms of the GNU General Public License as published by
  13         the Free Software Foundation, either version 3 of the License, or
  14         (at your option) any later version.
  15
  16         libc3 is distributed in the hope that it will be useful,
  17         but WITHOUT ANY WARRANTY; without even the implied warranty of
  18         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19         GNU General Public License for more details.
  20
  21         You should have received a copy of the GNU General Public License
  22         along with libc3.  If not, see <http://www.gnu.org/licenses/>.
  23  */
  24
  25 #include <math.h>
  26 #include <string.h>
  27 #include "c3algebra.h"
  28
  29 #ifndef MAX
  30 #define MAX(a,b)  ((a)>(b) ? (a) : (b))
  31 #define MIN(a,b)  ((a)<(b) ? (a) : (b))
  32 #endif
  33
  34 /****************************************************************
  35  *                                                              *
  36  *          c3vec2 Member functions                             *
  37  *                                                              *
  38  ****************************************************************/
  39
  40 /******************** c3vec2 CONSTRUCTORS ********************/
  41
  42 c3vec2 c3vec2_zero()
  43 {
  44         c3vec2 n;// = { .x = 0, .y = 0 };// older gcc <4.6 doesn't like this
  45         n.x = n.y = 0;
  46     return n;
  47 }
  48
  49 c3vec2 c3vec2f(c3f x, c3f y)
  50 {
  51         c3vec2 v;// = { .x = x, .y = y };// older gcc <4.6 doesn't like this
  52         v.x = x; v.y = y;
  53         return v;
  54 }
  55
  56 /******************** c3vec2 ASSIGNMENT OPERATORS ******************/
  57
  58 c3vec2  c3vec2_add(c3vec2 a, const c3vec2 v)
  59 {
  60     a.n[VX] += v.n[VX];
  61     a.n[VY] += v.n[VY];
  62     return a;
  63 }
  64
  65 c3vec2  c3vec2_sub(c3vec2 a, const c3vec2 v)
  66 {
  67     a.n[VX] -= v.n[VX];
  68     a.n[VY] -= v.n[VY];
  69     return a;
  70 }
  71
  72 c3vec2 c3vec2_mulf(c3vec2 a, c3f d)
  73 {
  74     a.n[VX] *= d;
  75     a.n[VY] *= d;
  76     return a;
  77 }
  78
  79 c3vec2 c3vec2_divf(c3vec2 a, c3f d)
  80 {
  81     c3f d_inv = 1.0f/d;
  82     a.n[VX] *= d_inv;
  83     a.n[VY] *= d_inv;
  84     return a;
  85 }
  86
  87 /******************** c3vec2 SPECIAL FUNCTIONS ********************/
  88
  89
  90 c3f c3vec2_length2(const c3vec2 a)
  91 {
  92     return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY];
  93 }
  94
  95 c3f c3vec2_length(const c3vec2 a)
  96 {
  97     return (c3f) sqrt(c3vec2_length2(a));
  98 }
  99
 100 c3vec2 c3vec2_normalize(const c3vec2 a) // it is up to caller to avoid divide-by-zero
 101 {
 102     return c3vec2_divf(a, c3vec2_length(a));
 103 }
 104
 105 c3vec2 c3vec2_apply(c3vec2 a, V_FCT_PTR fct)
 106 {
 107     a.n[VX] = fct(a.n[VX]);
 108     a.n[VY] = fct(a.n[VY]);
 109     return a;
 110 }
 111
 112
 113 /******************** c3vec2 FRIENDS *****************************/
 114
 115 c3vec2 c3vec2_minus(const c3vec2 a)
 116 {
 117     return c3vec2f(-a.n[VX],-a.n[VY]);
 118 }
 119
 120 c3vec2 c3mat3_mulv2(const c3mat3p a, const c3vec2 v)
 121 {
 122   c3vec2 av;
 123
 124   av.n[VX] = a->v[0].n[VX]*v.n[VX] + a->v[0].n[VY]*v.n[VY] + a->v[0].n[VZ];
 125   av.n[VY] = a->v[1].n[VX]*v.n[VX] + a->v[1].n[VY]*v.n[VY] + a->v[1].n[VZ];
 126 //  av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
 127
 128   return av;
 129 }
 130
 131 c3vec2 c3vec2_mulm3(const c3vec2 v, const c3mat3p a)
 132 {
 133         c3mat3 t = c3mat3_transpose(a);
 134     return c3mat3_mulv2(&t, v);
 135 }
 136
 137 c3vec3 c3mat3_mulv3(const c3mat3p a, const c3vec3 v)
 138 {
 139     c3vec3 av;
 140
 141     av.n[VX] = a->v[0].n[VX]*v.n[VX] + a->v[0].n[VY]*v.n[VY] + a->v[0].n[VZ]*v.n[VZ];
 142     av.n[VY] = a->v[1].n[VX]*v.n[VX] + a->v[1].n[VY]*v.n[VY] + a->v[1].n[VZ]*v.n[VZ];
 143     av.n[VZ] = a->v[2].n[VX]*v.n[VX] + a->v[2].n[VY]*v.n[VY] + a->v[2].n[VZ]*v.n[VZ];
 144
 145     return av;
 146 }
 147
 148 c3vec3 c3vec3_mulm3(const c3vec3 v, const c3mat3p a)
 149 {
 150         c3mat3 t = c3mat3_transpose(a);
 151     return c3mat3_mulv3(&t, v);
 152 }
 153
 154 c3f c3vec2_dot(const c3vec2 a, const c3vec2 b)
 155 {
 156     return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY];
 157 }
 158
 159 c3vec3 c3vec2_cross(const c3vec2 a, const c3vec2 b)
 160 {
 161     return c3vec3f(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]);
 162 }
 163
 164 int c3vec2_equal(const c3vec2 a, const c3vec2 b)
 165 {
 166     return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]);
 167 }
 168
 169
 170 c3vec2 c3vec2_min(const c3vec2 a, const c3vec2 b)
 171 {
 172     return c3vec2f(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]));
 173 }
 174
 175 c3vec2 c3vec2_max(const c3vec2 a, const c3vec2 b)
 176 {
 177     return c3vec2f(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]));
 178 }
 179
 180 c3vec2 c3vec2_prod(const c3vec2 a, const c3vec2 b)
 181 {
 182     return c3vec2f(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]);
 183 }
 184
 185 /****************************************************************
 186  *                                                              *
 187  *          c3vec3 Member functions                               *
 188  *                                                              *
 189  ****************************************************************/
 190
 191 // CONSTRUCTORS
 192
 193 c3vec3 c3vec3_zero()
 194 {
 195         c3vec3 n;// = { .x = 0, .y = 0, .z = 0 };// older gcc <4.6 doesn't like this
 196         n.x = n.y = n.z = 0;
 197     return n;
 198 }
 199
 200 c3vec3 c3vec3f(c3f x, c3f y, c3f z)
 201 {
 202         c3vec3 v;// = { .x = x, .y = y, .z = z };// older gcc <4.6 doesn't like this
 203         v.x = x; v.y = y; v.z = z;
 204         return v;
 205 }
 206
 207 c3vec3 c3vec3_vec2f(const c3vec2 v, c3f d)
 208 {
 209         c3vec3 n;// = { .x = v.x, .y = v.y, .z = d }; // older gcc <4.6 doesn't like this
 210         n.x = v.x; n.y = v.y; n.z = d;
 211         return n;
 212 }
 213
 214 c3vec3 c3vec3_vec2(const c3vec2 v)
 215 {
 216         return c3vec3_vec2f(v, 1.0);
 217 }
 218
 219 c3vec3 c3vec3_vec4(const c3vec4 v) // it is up to caller to avoid divide-by-zero
 220 {
 221         c3vec3 n;
 222     n.n[VX] = v.n[VX] / v.n[VW];
 223     n.n[VY] = v.n[VY] / v.n[VW];
 224     n.n[VZ] = v.n[VZ] / v.n[VW];
 225     return n;
 226 }
 227
 228
 229 c3vec3 c3vec3_add(c3vec3 a, const c3vec3 v)
 230 {
 231     a.n[VX] += v.n[VX];
 232     a.n[VY] += v.n[VY];
 233     a.n[VZ] += v.n[VZ];
 234     return a;
 235 }
 236
 237 c3vec3 c3vec3_sub(c3vec3 a, const c3vec3 v)
 238 {
 239         a.n[VX] -= v.n[VX];
 240         a.n[VY] -= v.n[VY];
 241         a.n[VZ] -= v.n[VZ];
 242     return a;
 243 }
 244
 245 c3vec3 c3vec3_mulf(c3vec3 a, c3f d)
 246 {
 247         a.n[VX] *= d;
 248         a.n[VY] *= d;
 249         a.n[VZ] *= d;
 250     return a;
 251 }
 252
 253 c3vec3 c3vec3_divf(c3vec3 a, c3f d)
 254 {
 255     c3f d_inv = 1.0f/d;
 256     a.n[VX] *= d_inv;
 257     a.n[VY] *= d_inv;
 258     a.n[VZ] *= d_inv;
 259     return a;
 260 }
 261
 262 // SPECIAL FUNCTIONS
 263
 264 c3f c3vec3_length2(const c3vec3 a)
 265 {
 266     return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY] + a.n[VZ]*a.n[VZ];
 267 }
 268
 269 c3f c3vec3_length(const c3vec3 a)
 270 {
 271     return (c3f) sqrt(c3vec3_length2(a));
 272 }
 273
 274 c3vec3 c3vec3_normalize(const c3vec3 a) // it is up to caller to avoid divide-by-zero
 275 {
 276     return c3vec3_divf(a, c3vec3_length(a));
 277 }
 278
 279 c3vec3 c3vec3_homogenize(c3vec3 a) // it is up to caller to avoid divide-by-zero
 280 {
 281     a.n[VX] /= a.n[VZ];
 282     a.n[VY] /= a.n[VZ];
 283     a.n[VZ] = 1.0;
 284     return a;
 285 }
 286
 287 c3vec3 c3vec3_apply(c3vec3 a, V_FCT_PTR fct)
 288 {
 289     a.n[VX] = fct(a.n[VX]);
 290     a.n[VY] = fct(a.n[VY]);
 291     a.n[VZ] = fct(a.n[VZ]);
 292     return a;
 293 }
 294
 295 // FRIENDS
 296
 297 c3vec3 c3vec3_minus(const c3vec3 a)
 298 {
 299     return c3vec3f(-a.n[VX],-a.n[VY],-a.n[VZ]);
 300 }
 301
 302 #if later
 303 c3vec3 operator*(const c3mat4 &a, const c3vec3 &v)
 304 {
 305     return a*c3vec4(v);
 306 }
 307
 308 c3vec3 operator*(const c3vec3 &v, c3mat4 &a)
 309 {
 310     return a.transpose()*v;
 311 }
 312 #endif
 313
 314 c3f c3vec3_dot(const c3vec3 a, const c3vec3 b)
 315 {
 316     return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ];
 317 }
 318
 319 c3vec3 c3vec3_cross(const c3vec3 a, const c3vec3 b)
 320 {
 321     return
 322         c3vec3f(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY],
 323              a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ],
 324              a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]);
 325 }
 326
 327 int c3vec3_equal(const c3vec3 a, const c3vec3 b)
 328 {
 329     return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]);
 330 }
 331
 332
 333 c3vec3 c3vec3_min(const c3vec3 a, const c3vec3 b)
 334 {
 335     return c3vec3f(
 336         MIN(a.n[VX], b.n[VX]),
 337         MIN(a.n[VY], b.n[VY]),
 338         MIN(a.n[VZ], b.n[VZ]));
 339 }
 340
 341 c3vec3 c3vec3_max(const c3vec3 a, const c3vec3 b)
 342 {
 343     return c3vec3f(
 344         MAX(a.n[VX], b.n[VX]),
 345         MAX(a.n[VY], b.n[VY]),
 346         MAX(a.n[VZ], b.n[VZ]));
 347 }
 348
 349 c3vec3 c3vec3_prod(const c3vec3 a, const c3vec3 b)
 350 {
 351     return c3vec3f(a.n[VX]*b.n[VX], a.n[VY]*b.n[VY], a.n[VZ]*b.n[VZ]);
 352 }
 353
 354 c3vec3 c3vec3_polar(const c3vec3 a)
 355 {
 356         c3f l = c3vec3_length(a);
 357         c3f phi = atan2(a.y, a.x);
 358         c3f theta = acos(a.z / l);
 359         return c3vec3f(phi, theta, l);
 360 }
 361
 362 /****************************************************************
 363  *                                                              *
 364  *          c3vec4 Member functions                               *
 365  *                                                              *
 366  ****************************************************************/
 367
 368 // CONSTRUCTORS
 369
 370 c3vec4 c3vec4_zero()
 371 {
 372         c3vec4 n ;//= { .x = 0, .y = 0, .z = 0, .w = 1.0 }; // older gcc <4.6 doesn't like this
 373         n.x = n.y = n.z = 0; n.w = 1.0;
 374     return n;
 375 }
 376
 377 c3vec4 c3vec4f(c3f x, c3f y, c3f z, c3f w)
 378 {
 379         c3vec4 n;// = { .x = x, .y = y, .z = z, .w = w }; // older gcc <4.6 doesn't like this
 380         n.x =x;  n.y = y; n.z = z; n.w = w;
 381     return n;
 382 }
 383
 384 c3vec4 c3vec4_vec3(const c3vec3 v)
 385 {
 386         return c3vec4f(v.n[VX], v.n[VY], v.n[VZ], 1.0);
 387 }
 388
 389 c3vec4 c3vec4_vec3f(const c3vec3 v, c3f d)
 390 {
 391         return c3vec4f(v.n[VX], v.n[VY], v.n[VZ], d);
 392 }
 393
 394 // ASSIGNMENT OPERATORS
 395
 396 c3vec4 c3vec4_add(c3vec4 a, const c3vec4 v)
 397 {
 398     a.n[VX] += v.n[VX];
 399     a.n[VY] += v.n[VY];
 400     a.n[VZ] += v.n[VZ];
 401     a.n[VW] += v.n[VW];
 402     return a;
 403 }
 404
 405 c3vec4 c3vec4_sub(c3vec4 a, const c3vec4 v)
 406 {
 407         a.n[VX] -= v.n[VX];
 408         a.n[VY] -= v.n[VY];
 409         a.n[VZ] -= v.n[VZ];
 410         a.n[VW] -= v.n[VW];
 411     return a;
 412 }
 413
 414 c3vec4 c3vec4_mulf(c3vec4 a, c3f d)
 415 {
 416     a.n[VX] *= d;
 417     a.n[VY] *= d;
 418     a.n[VZ] *= d;
 419     a.n[VW] *= d;
 420     return a;
 421 }
 422
 423 c3vec4 c3vec4_divf(c3vec4 a, c3f d)
 424 {
 425     c3f d_inv = 1.0f/d;
 426     a.n[VX] *= d_inv;
 427     a.n[VY] *= d_inv;
 428     a.n[VZ] *= d_inv;
 429     a.n[VW] *= d_inv;
 430     return a;
 431 }
 432
 433 // SPECIAL FUNCTIONS
 434
 435 c3f c3vec4_length2(const c3vec4 a)
 436 {
 437     return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY] + a.n[VZ]*a.n[VZ] + a.n[VW]*a.n[VW];
 438 }
 439
 440 c3f c3vec4_length(const c3vec4 a)
 441 {
 442     return (c3f) sqrt(c3vec4_length2(a));
 443 }
 444
 445 c3vec4 c3vec4_normalize(c3vec4 a) // it is up to caller to avoid divide-by-zero
 446 {
 447     return c3vec4_divf(a, c3vec4_length(a));
 448 }
 449
 450 c3vec4 c3vec4_homogenize(c3vec4 a) // it is up to caller to avoid divide-by-zero
 451 {
 452     a.n[VX] /= a.n[VW];
 453     a.n[VY] /= a.n[VW];
 454     a.n[VZ] /= a.n[VW];
 455     a.n[VW] = 1.0;
 456     return a;
 457 }
 458
 459 c3vec4 c3vec4_apply(c3vec4 a, V_FCT_PTR fct)
 460 {
 461     a.n[VX] = fct(a.n[VX]);
 462     a.n[VY] = fct(a.n[VY]);
 463     a.n[VZ] = fct(a.n[VZ]);
 464     a.n[VW] = fct(a.n[VW]);
 465     return a;
 466 }
 467
 468 c3vec4 c3mat4_mulv4(const c3mat4p a, const c3vec4 v)
 469 {
 470     #define ROWCOL(i) \
 471         a->v[i].n[0]*v.n[VX] + \
 472         a->v[i].n[1]*v.n[VY] + \
 473         a->v[i].n[2]*v.n[VZ] + \
 474         a->v[i].n[3]*v.n[VW]
 475
 476     return c3vec4f(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
 477
 478     #undef ROWCOL
 479 }
 480
 481 c3vec4 c3vec4_mulm4(const c3vec4 v, const c3mat4p a)
 482 {
 483         c3mat4 m = c3mat4_transpose(a);
 484     return c3mat4_mulv4(&m, v);
 485 }
 486
 487 c3vec3 c3mat4_mulv3(const c3mat4p a, const c3vec3 v)
 488 {
 489         return c3vec3_vec4(c3mat4_mulv4(a, c3vec4_vec3(v)));
 490 }
 491
 492 c3vec4 c3vec4_minus(const c3vec4 a)
 493 {
 494     return c3vec4f(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]);
 495 }
 496
 497 int c3vec4_equal(const c3vec4 a, const c3vec4 b)
 498 {
 499     return
 500         (a.n[VX] == b.n[VX]) &&
 501         (a.n[VY] == b.n[VY]) &&
 502         (a.n[VZ] == b.n[VZ]) &&
 503         (a.n[VW] == b.n[VW]);
 504 }
 505
 506 c3vec4 c3vec4_min(const c3vec4 a, const c3vec4 b)
 507 {
 508     return c3vec4f(
 509         MIN(a.n[VX], b.n[VX]),
 510         MIN(a.n[VY], b.n[VY]),
 511         MIN(a.n[VZ], b.n[VZ]),
 512         MIN(a.n[VW], b.n[VW]));
 513 }
 514
 515 c3vec4 c3vec4_max(const c3vec4 a, const c3vec4 b)
 516 {
 517     return c3vec4f(
 518         MAX(a.n[VX], b.n[VX]),
 519         MAX(a.n[VY], b.n[VY]),
 520         MAX(a.n[VZ], b.n[VZ]),
 521         MAX(a.n[VW], b.n[VW]));
 522 }
 523
 524 c3vec4 c3vec4_prod(const c3vec4 a, const c3vec4 b)
 525 {
 526     return c3vec4f(
 527         a.n[VX] * b.n[VX],
 528         a.n[VY] * b.n[VY],
 529         a.n[VZ] * b.n[VZ],
 530         a.n[VW] * b.n[VW]);
 531 }
 532
 533 /****************************************************************
 534  *                                                              *
 535  *          c3mat3 member functions                               *
 536  *                                                              *
 537  ****************************************************************/
 538
 539 // CONSTRUCTORS
 540
 541 c3mat3 c3mat3_identity()
 542 {
 543         return identity2D();
 544 }
 545
 546 c3mat3 c3mat3_vec3(const c3vec3 v0, const c3vec3 v1, const c3vec3 v2)
 547 {
 548         c3mat3 m = { .v[0] = v0, .v[1] = v1, .v[2] = v2 };
 549         return m;
 550 }
 551
 552 c3mat3p c3mat3_add(const c3mat3p a, const c3mat3p m)
 553 {
 554         a->v[0] = c3vec3_add(a->v[0], m->v[0]);
 555         a->v[1] = c3vec3_add(a->v[1], m->v[1]);
 556         a->v[2] = c3vec3_add(a->v[2], m->v[2]);
 557     return a;
 558 }
 559
 560 c3mat3p c3mat3_sub(const c3mat3p a, const c3mat3p m)
 561 {
 562         a->v[0] = c3vec3_sub(a->v[0], m->v[0]);
 563         a->v[1] = c3vec3_sub(a->v[1], m->v[1]);
 564         a->v[2] = c3vec3_sub(a->v[2], m->v[2]);
 565     return a;
 566 }
 567
 568 c3mat3p c3mat3_mulf(const c3mat3p a, c3f d)
 569 {
 570         a->v[0] = c3vec3_mulf(a->v[0], d);
 571         a->v[1] = c3vec3_mulf(a->v[1], d);
 572         a->v[2] = c3vec3_mulf(a->v[2], d);
 573     return a;
 574 }
 575
 576 c3mat3p c3mat3_divf(const c3mat3p a, c3f d)
 577 {
 578         a->v[0] = c3vec3_divf(a->v[0], d);
 579         a->v[1] = c3vec3_divf(a->v[1], d);
 580         a->v[2] = c3vec3_divf(a->v[2], d);
 581     return a;
 582 }
 583
 584 // SPECIAL FUNCTIONS
 585
 586 c3mat3 c3mat3_transpose(const c3mat3p a)
 587 {
 588     return c3mat3_vec3(
 589         c3vec3f(a->v[0].n[0], a->v[1].n[0], a->v[2].n[0]),
 590         c3vec3f(a->v[0].n[1], a->v[1].n[1], a->v[2].n[1]),
 591         c3vec3f(a->v[0].n[2], a->v[1].n[2], a->v[2].n[2]));
 592 }
 593
 594 c3mat3 c3mat3_inverse(const c3mat3p m)   // Gauss-Jordan elimination with partial pivoting
 595 {
 596         c3mat3 a = *m; // As a evolves from original mat into identity
 597         c3mat3 b = c3mat3_identity(); // b evolves from identity into inverse(a)
 598         int i, j, i1;
 599
 600         // Loop over cols of a from left to right, eliminating above and below diag
 601         for (j = 0; j < 3; j++) { // Find largest pivot in column j among rows j..2
 602                 i1 = j; // Row with largest pivot candidate
 603                 for (i = j + 1; i < 3; i++)
 604                         if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
 605                                 i1 = i;
 606
 607                 // Swap rows i1 and j in a and b to put pivot on diagonal
 608                 c3vec3 _s;
 609                 _s = a.v[i1]; a.v[i1] = a.v[j]; a.v[j] = _s;  // swap(a.v[i1], a.v[j]);
 610                 _s = b.v[i1]; b.v[i1] = b.v[j]; b.v[j] = _s;  //swap(b.v[i1], b.v[j]);
 611
 612                 // Scale row j to have a unit diagonal
 613                 if (a.v[j].n[j] == 0.) {
 614                 //      VEC_ERROR("c3mat3::inverse: singular matrix; can't invert\n");
 615                         return *m;
 616                 }
 617
 618                 b.v[j] = c3vec3_divf(b.v[j], a.v[j].n[j]);
 619                 a.v[j] = c3vec3_divf(a.v[j], a.v[j].n[j]);
 620
 621                 // Eliminate off-diagonal elems in col j of a, doing identical ops to b
 622                 for (i = 0; i < 3; i++)
 623                         if (i != j) {
 624                                 b.v[i] = c3vec3_sub(b.v[i], c3vec3_mulf(b.v[j], a.v[i].n[j]));
 625                                 a.v[i] = c3vec3_sub(a.v[i], c3vec3_mulf(a.v[j], a.v[i].n[j]));
 626                         }
 627         }
 628
 629         return b;
 630 }
 631
 632 c3mat3p c3mat3_apply(c3mat3p a, V_FCT_PTR fct)
 633 {
 634         a->v[0] = c3vec3_apply(a->v[0], fct);
 635         a->v[1] = c3vec3_apply(a->v[1], fct);
 636         a->v[2] = c3vec3_apply(a->v[2], fct);
 637     return a;
 638 }
 639
 640
 641 c3mat3 c3mat3_minus(const c3mat3p a)
 642 {
 643     return c3mat3_vec3(
 644                 c3vec3_minus(a->v[0]),
 645                 c3vec3_minus(a->v[1]),
 646                 c3vec3_minus(a->v[2]));
 647 }
 648
 649 c3mat3 c3mat3_mul(const c3mat3p a, const c3mat3p b)
 650 {
 651     #define ROWCOL(i, j) \
 652     a->v[i].n[0]*b->v[0].n[j] + a->v[i].n[1]*b->v[1].n[j] + a->v[i].n[2]*b->v[2].n[j]
 653
 654     return c3mat3_vec3(
 655         c3vec3f(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
 656         c3vec3f(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
 657         c3vec3f(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
 658
 659     #undef ROWCOL
 660 }
 661
 662 int c3mat3_equal(const c3mat3p a, const c3mat3p b)
 663 {
 664     return
 665         c3vec3_equal(a->v[0], b->v[0]) &&
 666         c3vec3_equal(a->v[1], b->v[1]) &&
 667         c3vec3_equal(a->v[2], b->v[2]);
 668 }
 669
 670 /****************************************************************
 671  *                                                              *
 672  *          c3mat4 member functions                               *
 673  *                                                              *
 674  ****************************************************************/
 675
 676 // CONSTRUCTORS
 677
 678 c3mat4 c3mat4_identity()
 679 {
 680     return identity3D();
 681 }
 682
 683 c3mat4 c3mat4_vec4(const c3vec4 v0, const c3vec4 v1, const c3vec4 v2, const c3vec4 v3)
 684 {
 685         c3mat4 m = { .v[0] = v0, .v[1] = v1, .v[2] = v2, .v[3] = v3 };
 686         return m;
 687 }
 688
 689 c3mat4 c3mat4f(
 690      c3f a00, c3f a01, c3f a02, c3f a03,
 691      c3f a10, c3f a11, c3f a12, c3f a13,
 692      c3f a20, c3f a21, c3f a22, c3f a23,
 693      c3f a30, c3f a31, c3f a32, c3f a33 )
 694 {
 695         c3mat4 m;
 696         m.v[0] = c3vec4f(a00, a01, a01, a03);
 697         m.v[1] = c3vec4f(a10, a11, a11, a13);
 698         m.v[2] = c3vec4f(a20, a21, a21, a23);
 699         m.v[3] = c3vec4f(a30, a31, a21, a33);
 700         return m;
 701 }
 702
 703 c3mat4p c3mat4p_add(c3mat4p a, const c3mat4p m)
 704 {
 705     a->v[0] = c3vec4_add(a->v[0], m->v[0]);
 706     a->v[1] = c3vec4_add(a->v[1], m->v[1]);
 707     a->v[2] = c3vec4_add(a->v[2], m->v[2]);
 708     a->v[3] = c3vec4_add(a->v[3], m->v[3]);
 709     return a;
 710 }
 711
 712 c3mat4p c3mat4p_sub(c3mat4p a, const c3mat4p m)
 713 {
 714     a->v[0] = c3vec4_sub(a->v[0], m->v[0]);
 715     a->v[1] = c3vec4_sub(a->v[1], m->v[1]);
 716     a->v[2] = c3vec4_sub(a->v[2], m->v[2]);
 717     a->v[3] = c3vec4_sub(a->v[3], m->v[3]);
 718     return a;
 719 }
 720
 721 c3mat4p c3mat4p_mulf(c3mat4p a, c3f d)
 722 {
 723     a->v[0] = c3vec4_mulf(a->v[0], d);
 724     a->v[1] = c3vec4_mulf(a->v[1], d);
 725     a->v[2] = c3vec4_mulf(a->v[2], d);
 726     a->v[3] = c3vec4_mulf(a->v[3], d);
 727     return a;
 728 }
 729
 730 c3mat4p c3mat4p_divf(c3mat4p a, c3f d)
 731 {
 732     a->v[0] = c3vec4_divf(a->v[0], d);
 733     a->v[1] = c3vec4_divf(a->v[1], d);
 734     a->v[2] = c3vec4_divf(a->v[2], d);
 735     a->v[3] = c3vec4_divf(a->v[3], d);
 736     return a;
 737 }
 738
 739 // SPECIAL FUNCTIONS;
 740
 741 c3mat4 c3mat4_transpose(const c3mat4p a)
 742 {
 743     return c3mat4_vec4(
 744         c3vec4f(a->v[0].n[0], a->v[1].n[0], a->v[2].n[0], a->v[3].n[0]),
 745         c3vec4f(a->v[0].n[1], a->v[1].n[1], a->v[2].n[1], a->v[3].n[1]),
 746         c3vec4f(a->v[0].n[2], a->v[1].n[2], a->v[2].n[2], a->v[3].n[2]),
 747         c3vec4f(a->v[0].n[3], a->v[1].n[3], a->v[2].n[3], a->v[3].n[3]));
 748 }
 749
 750 c3mat4 c3mat4_inverse(const c3mat4p m)       // Gauss-Jordan elimination with partial pivoting
 751 {
 752         c3mat4 a = *m; // As a evolves from original mat into identity
 753         c3mat4 b = identity3D(); // b evolves from identity into inverse(a)
 754         int i, j, i1;
 755
 756         // Loop over cols of a from left to right, eliminating above and below diag
 757         for (j = 0; j < 4; j++) { // Find largest pivot in column j among rows j..3
 758                 i1 = j; // Row with largest pivot candidate
 759                 for (i = j + 1; i < 4; i++)
 760                         if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
 761                                 i1 = i;
 762
 763                 // Swap rows i1 and j in a and b to put pivot on diagonal
 764                 c3vec4 _s;
 765                 _s = a.v[i1]; a.v[i1] = a.v[j]; a.v[j] = _s; // swap(a.v[i1], a.v[j]);
 766                 _s = b.v[i1]; b.v[i1] = b.v[j]; b.v[j] = _s; // swap(b.v[i1], b.v[j]);
 767
 768                 // Scale row j to have a unit diagonal
 769                 if (a.v[j].n[j] == 0.) {
 770                         //    VEC_ERROR("c3mat4::inverse: singular matrix; can't invert\n");
 771                         return a;
 772                 }
 773                 b.v[j] = c3vec4_divf(b.v[j], a.v[j].n[j]);
 774                 a.v[j] = c3vec4_divf(a.v[j], a.v[j].n[j]);
 775
 776                 // Eliminate off-diagonal elems in col j of a, doing identical ops to b
 777                 for (i = 0; i < 4; i++)
 778                         if (i != j) {
 779                                 b.v[i] = c3vec4_sub(b.v[i], c3vec4_mulf(b.v[j], a.v[i].n[j]));
 780                                 a.v[i] = c3vec4_sub(a.v[i], c3vec4_mulf(a.v[j], a.v[i].n[j]));
 781                         }
 782         }
 783
 784         return b;
 785 }
 786
 787 c3mat4p c3mat4p_apply(c3mat4p a, V_FCT_PTR fct)
 788 {
 789         a->v[0] = c3vec4_apply(a->v[0], fct);
 790         a->v[1] = c3vec4_apply(a->v[1], fct);
 791         a->v[2] = c3vec4_apply(a->v[2], fct);
 792         a->v[3] = c3vec4_apply(a->v[3], fct);
 793     return a;
 794 }
 795
 796 c3mat4p c3mat4p_swap_rows(c3mat4p a, int i, int j)
 797 {
 798     c3vec4 t;
 799
 800     t    = a->v[i];
 801     a->v[i] = a->v[j];
 802     a->v[j] = t;
 803     return a;
 804 }
 805
 806 c3mat4p c3mat4p_swap_cols(c3mat4p a, int i, int j)
 807 {
 808         c3f t;
 809
 810         for (int k = 0; k < 4; k++) {
 811                 t = a->v[k].n[i];
 812                 a->v[k].n[i] = a->v[k].n[j];
 813                 a->v[k].n[j] = t;
 814         }
 815         return a;
 816 }
 817
 818
 819 // FRIENDS
 820
 821 c3mat4 c3mat4_minus(const c3mat4p a)
 822 {
 823     return c3mat4_vec4(
 824                 c3vec4_minus(a->v[0]),
 825                 c3vec4_minus(a->v[1]),
 826                 c3vec4_minus(a->v[2]),
 827                 c3vec4_minus(a->v[3]));
 828 }
 829
 830 c3mat4 c3mat4_add(const c3mat4p a, const c3mat4p b)
 831 {
 832     return c3mat4_vec4(
 833         c3vec4_add(a->v[0], b->v[0]),
 834         c3vec4_add(a->v[1], b->v[1]),
 835         c3vec4_add(a->v[2], b->v[2]),
 836         c3vec4_add(a->v[3], b->v[3]));
 837 }
 838
 839 c3mat4 c3mat4_sub(const c3mat4p a, const c3mat4p b)
 840 {
 841     return c3mat4_vec4(
 842         c3vec4_sub(a->v[0], b->v[0]),
 843         c3vec4_sub(a->v[1], b->v[1]),
 844         c3vec4_sub(a->v[2], b->v[2]),
 845         c3vec4_sub(a->v[3], b->v[3]));
 846 }
 847
 848 c3mat4 c3mat4_mul(const c3mat4p a, const c3mat4p b)
 849 {
 850     #define ROWCOL(i, j) \
 851         a->v[i].n[0]*b->v[0].n[j] + \
 852         a->v[i].n[1]*b->v[1].n[j] + \
 853         a->v[i].n[2]*b->v[2].n[j] + \
 854         a->v[i].n[3]*b->v[3].n[j]
 855
 856     return c3mat4_vec4(
 857         c3vec4f(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
 858         c3vec4f(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
 859         c3vec4f(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
 860         c3vec4f(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
 861         );
 862
 863     #undef ROWCOL
 864 }
 865
 866 c3mat4 c3mat4_mulf(const c3mat4p a, c3f d)
 867 {
 868         c3mat4 r = *a;
 869         return *c3mat4p_mulf(&r, d);
 870 }
 871
 872 c3mat4 c3mat4_divf(const c3mat4p a, c3f d)
 873 {
 874         c3mat4 r = *a;
 875         return *c3mat4p_divf(&r, d);
 876 }
 877
 878 int c3mat4_equal(const c3mat4p a, const c3mat4p b)
 879 {
 880         return !memcmp(a->n, b->n, sizeof(a->n));
 881 #if 0
 882     return
 883         c3vec4_equal(a->v[0], b->v[0]) &&
 884         c3vec4_equal(a->v[1], b->v[1]) &&
 885         c3vec4_equal(a->v[2], b->v[2]) &&
 886         c3vec4_equal(a->v[3], b->v[3]);
 887 #endif
 888 }
 889
 890 /****************************************************************
 891  *                                                              *
 892  *         2D functions and 3D functions                        *
 893  *                                                              *
 894  ****************************************************************/
 895
 896 c3mat3 identity2D()
 897 {
 898     return c3mat3_vec3(
 899         c3vec3f(1.0, 0.0, 0.0),
 900         c3vec3f(0.0, 1.0, 0.0),
 901         c3vec3f(0.0, 0.0, 1.0));
 902 }
 903
 904 c3mat3 translation2D(const c3vec2 v)
 905 {
 906     return c3mat3_vec3(
 907         c3vec3f(1.0, 0.0, v.n[VX]),
 908         c3vec3f(0.0, 1.0, v.n[VY]),
 909         c3vec3f(0.0, 0.0, 1.0));
 910 }
 911
 912 c3mat3 rotation2D(const c3vec2 Center, c3f angleDeg)
 913 {
 914     c3f angleRad = (c3f) (angleDeg * M_PI / 180.0);
 915     c3f c = (c3f) cos(angleRad);
 916     c3f s = (c3f) sin(angleRad);
 917
 918     return c3mat3_vec3(
 919         c3vec3f(c,    -s, Center.n[VX] * (1.0f-c) + Center.n[VY] * s),
 920         c3vec3f(s,     c, Center.n[VY] * (1.0f-c) - Center.n[VX] * s),
 921         c3vec3f(0.0, 0.0, 1.0));
 922 }
 923
 924 c3mat3 scaling2D(const c3vec2 scaleVector)
 925 {
 926     return c3mat3_vec3(
 927         c3vec3f(scaleVector.n[VX], 0.0, 0.0),
 928         c3vec3f(0.0, scaleVector.n[VY], 0.0),
 929         c3vec3f(0.0, 0.0, 1.0));
 930 }
 931
 932 c3mat4 identity3D()
 933 {
 934     return c3mat4_vec4(
 935         c3vec4f(1.0, 0.0, 0.0, 0.0),
 936         c3vec4f(0.0, 1.0, 0.0, 0.0),
 937         c3vec4f(0.0, 0.0, 1.0, 0.0),
 938         c3vec4f(0.0, 0.0, 0.0, 1.0));
 939 }
 940
 941 c3mat4 translation3D(const c3vec3 v)
 942 {
 943     return c3mat4_vec4(
 944         c3vec4f(1.0, 0.0, 0.0, v.n[VX]),
 945         c3vec4f(0.0, 1.0, 0.0, v.n[VY]),
 946         c3vec4f(0.0, 0.0, 1.0, v.n[VZ]),
 947         c3vec4f(0.0, 0.0, 0.0, 1.0));
 948 }
 949
 950 c3mat4 rotation3D(const c3vec3 Axis, c3f angleDeg)
 951 {
 952     c3f angleRad = (c3f) (angleDeg * M_PI / 180.0);
 953     c3f c = (c3f) cos(angleRad);
 954     c3f s = (c3f) sin(angleRad);
 955     c3f t = 1.0f - c;
 956
 957     c3vec3 axis = c3vec3_normalize(Axis);
 958
 959     return c3mat4_vec4(
 960         c3vec4f(t * axis.n[VX] * axis.n[VX] + c,
 961              t * axis.n[VX] * axis.n[VY] - s * axis.n[VZ],
 962              t * axis.n[VX] * axis.n[VZ] + s * axis.n[VY],
 963              0.0),
 964         c3vec4f(t * axis.n[VX] * axis.n[VY] + s * axis.n[VZ],
 965              t * axis.n[VY] * axis.n[VY] + c,
 966              t * axis.n[VY] * axis.n[VZ] - s * axis.n[VX],
 967              0.0),
 968         c3vec4f(t * axis.n[VX] * axis.n[VZ] - s * axis.n[VY],
 969              t * axis.n[VY] * axis.n[VZ] + s * axis.n[VX],
 970              t * axis.n[VZ] * axis.n[VZ] + c,
 971              0.0),
 972         c3vec4f(0.0, 0.0, 0.0, 1.0));
 973 }
 974
 975 c3mat4 rotation3Drad(const c3vec3 Axis, c3f angleRad)
 976 {
 977     c3f c = (c3f) cos(angleRad);
 978     c3f s = (c3f) sin(angleRad);
 979     c3f t = 1.0f - c;
 980
 981     c3vec3 axis = c3vec3_normalize(Axis);
 982
 983     return c3mat4_vec4(
 984         c3vec4f(t * axis.n[VX] * axis.n[VX] + c,
 985              t * axis.n[VX] * axis.n[VY] - s * axis.n[VZ],
 986              t * axis.n[VX] * axis.n[VZ] + s * axis.n[VY],
 987              0.0),
 988         c3vec4f(t * axis.n[VX] * axis.n[VY] + s * axis.n[VZ],
 989              t * axis.n[VY] * axis.n[VY] + c,
 990              t * axis.n[VY] * axis.n[VZ] - s * axis.n[VX],
 991              0.0),
 992         c3vec4f(t * axis.n[VX] * axis.n[VZ] - s * axis.n[VY],
 993              t * axis.n[VY] * axis.n[VZ] + s * axis.n[VX],
 994              t * axis.n[VZ] * axis.n[VZ] + c,
 995              0.0),
 996         c3vec4f(0.0, 0.0, 0.0, 1.0));
 997 }
 998
 999 c3mat4 scaling3D(const c3vec3 scaleVector)
1000 {
1001     return c3mat4_vec4(
1002         c3vec4f(scaleVector.n[VX], 0.0, 0.0, 0.0),
1003         c3vec4f(0.0, scaleVector.n[VY], 0.0, 0.0),
1004         c3vec4f(0.0, 0.0, scaleVector.n[VZ], 0.0),
1005         c3vec4f(0.0, 0.0, 0.0, 1.0));
1006 }
1007
1008 c3mat4 perspective3D(c3f d)
1009 {
1010     return c3mat4_vec4(
1011         c3vec4f(1.0f, 0.0f, 0.0f,   0.0f),
1012         c3vec4f(0.0f, 1.0f, 0.0f,   0.0f),
1013         c3vec4f(0.0f, 0.0f, 1.0f,   0.0f),
1014         c3vec4f(0.0f, 0.0f, 1.0f/d, 0.0f));
1015 }
1016