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