vvector.h (35049B)
1 /* taken from 2 https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h 3 */ 4 5 /* 6 * vvector.h 7 * 8 * FUNCTION: 9 * This file contains a number of utilities useful for handling 10 * 3D vectors 11 * 12 * HISTORY: 13 * Written by Linas Vepstas, August 1991 14 * Added 2D code, March 1993 15 * Added Outer products, C++ proofed, Linas Vepstas October 1993 16 */ 17 18 #ifndef __GUTIL_VECTOR_H__ 19 #define __GUTIL_VECTOR_H__ 20 21 #if defined(__cplusplus) || defined(c_plusplus) 22 extern "C" { 23 #endif 24 25 26 #include <math.h> 27 // #include "port.h" 28 29 /* ========================================================== */ 30 /* Zero out a 2D vector */ 31 32 #define VEC_ZERO_2(a) \ 33 { \ 34 (a)[0] = (a)[1] = 0.0; \ 35 } 36 37 /* ========================================================== */ 38 /* Zero out a 3D vector */ 39 40 #define VEC_ZERO(a) \ 41 { \ 42 (a)[0] = (a)[1] = (a)[2] = 0.0; \ 43 } 44 45 /* ========================================================== */ 46 /* Zero out a 4D vector */ 47 48 #define VEC_ZERO_4(a) \ 49 { \ 50 (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0; \ 51 } 52 53 /* ========================================================== */ 54 /* Vector copy */ 55 56 #define VEC_COPY_2(b,a) \ 57 { \ 58 (b)[0] = (a)[0]; \ 59 (b)[1] = (a)[1]; \ 60 } 61 62 /* ========================================================== */ 63 /* Copy 3D vector */ 64 65 #define VEC_COPY(b,a) \ 66 { \ 67 (b)[0] = (a)[0]; \ 68 (b)[1] = (a)[1]; \ 69 (b)[2] = (a)[2]; \ 70 } 71 72 /* ========================================================== */ 73 /* Copy 4D vector */ 74 75 #define VEC_COPY_4(b,a) \ 76 { \ 77 (b)[0] = (a)[0]; \ 78 (b)[1] = (a)[1]; \ 79 (b)[2] = (a)[2]; \ 80 (b)[3] = (a)[3]; \ 81 } 82 83 /* ========================================================== */ 84 /* Vector difference */ 85 86 #define VEC_DIFF_2(v21,v2,v1) \ 87 { \ 88 (v21)[0] = (v2)[0] - (v1)[0]; \ 89 (v21)[1] = (v2)[1] - (v1)[1]; \ 90 } 91 92 /* ========================================================== */ 93 /* Vector difference */ 94 95 #define VEC_DIFF(v21,v2,v1) \ 96 { \ 97 (v21)[0] = (v2)[0] - (v1)[0]; \ 98 (v21)[1] = (v2)[1] - (v1)[1]; \ 99 (v21)[2] = (v2)[2] - (v1)[2]; \ 100 } 101 102 /* ========================================================== */ 103 /* Vector difference */ 104 105 #define VEC_DIFF_4(v21,v2,v1) \ 106 { \ 107 (v21)[0] = (v2)[0] - (v1)[0]; \ 108 (v21)[1] = (v2)[1] - (v1)[1]; \ 109 (v21)[2] = (v2)[2] - (v1)[2]; \ 110 (v21)[3] = (v2)[3] - (v1)[3]; \ 111 } 112 113 /* ========================================================== */ 114 /* Vector sum */ 115 116 #define VEC_SUM_2(v21,v2,v1) \ 117 { \ 118 (v21)[0] = (v2)[0] + (v1)[0]; \ 119 (v21)[1] = (v2)[1] + (v1)[1]; \ 120 } 121 122 /* ========================================================== */ 123 /* Vector sum */ 124 125 #define VEC_SUM(v21,v2,v1) \ 126 { \ 127 (v21)[0] = (v2)[0] + (v1)[0]; \ 128 (v21)[1] = (v2)[1] + (v1)[1]; \ 129 (v21)[2] = (v2)[2] + (v1)[2]; \ 130 } 131 132 /* ========================================================== */ 133 /* Vector sum */ 134 135 #define VEC_SUM_4(v21,v2,v1) \ 136 { \ 137 (v21)[0] = (v2)[0] + (v1)[0]; \ 138 (v21)[1] = (v2)[1] + (v1)[1]; \ 139 (v21)[2] = (v2)[2] + (v1)[2]; \ 140 (v21)[3] = (v2)[3] + (v1)[3]; \ 141 } 142 143 /* ========================================================== */ 144 /* scalar times vector */ 145 146 #define VEC_SCALE_2(c,a,b) \ 147 { \ 148 (c)[0] = (a)*(b)[0]; \ 149 (c)[1] = (a)*(b)[1]; \ 150 } 151 152 /* ========================================================== */ 153 /* scalar times vector */ 154 155 #define VEC_SCALE(c,a,b) \ 156 { \ 157 (c)[0] = (a)*(b)[0]; \ 158 (c)[1] = (a)*(b)[1]; \ 159 (c)[2] = (a)*(b)[2]; \ 160 } 161 162 /* ========================================================== */ 163 /* scalar times vector */ 164 165 #define VEC_SCALE_4(c,a,b) \ 166 { \ 167 (c)[0] = (a)*(b)[0]; \ 168 (c)[1] = (a)*(b)[1]; \ 169 (c)[2] = (a)*(b)[2]; \ 170 (c)[3] = (a)*(b)[3]; \ 171 } 172 173 /* ========================================================== */ 174 /* accumulate scaled vector */ 175 176 #define VEC_ACCUM_2(c,a,b) \ 177 { \ 178 (c)[0] += (a)*(b)[0]; \ 179 (c)[1] += (a)*(b)[1]; \ 180 } 181 182 /* ========================================================== */ 183 /* accumulate scaled vector */ 184 185 #define VEC_ACCUM(c,a,b) \ 186 { \ 187 (c)[0] += (a)*(b)[0]; \ 188 (c)[1] += (a)*(b)[1]; \ 189 (c)[2] += (a)*(b)[2]; \ 190 } 191 192 /* ========================================================== */ 193 /* accumulate scaled vector */ 194 195 #define VEC_ACCUM_4(c,a,b) \ 196 { \ 197 (c)[0] += (a)*(b)[0]; \ 198 (c)[1] += (a)*(b)[1]; \ 199 (c)[2] += (a)*(b)[2]; \ 200 (c)[3] += (a)*(b)[3]; \ 201 } 202 203 /* ========================================================== */ 204 /* Vector dot product */ 205 206 #define VEC_DOT_PRODUCT_2(c,a,b) \ 207 { \ 208 c = (a)[0]*(b)[0] + (a)[1]*(b)[1]; \ 209 } 210 211 /* ========================================================== */ 212 /* Vector dot product */ 213 214 #define VEC_DOT_PRODUCT(c,a,b) \ 215 { \ 216 c = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]; \ 217 } 218 219 /* ========================================================== */ 220 /* Vector dot product */ 221 222 #define VEC_DOT_PRODUCT_4(c,a,b) \ 223 { \ 224 c = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3] ; \ 225 } 226 227 /* ========================================================== */ 228 /* vector impact parameter (squared) */ 229 230 #define VEC_IMPACT_SQ(bsq,direction,position) \ 231 { \ 232 gleDouble len, llel; \ 233 VEC_DOT_PRODUCT (len, position, position); \ 234 VEC_DOT_PRODUCT (llel, direction, position); \ 235 bsq = len - llel*llel; \ 236 } 237 238 /* ========================================================== */ 239 /* vector impact parameter */ 240 241 #define VEC_IMPACT(bsq,direction,position) \ 242 { \ 243 VEC_IMPACT_SQ(bsq,direction,position); \ 244 bsq = sqrt (bsq); \ 245 } 246 247 /* ========================================================== */ 248 /* Vector length */ 249 250 #define VEC_LENGTH_2(len,a) \ 251 { \ 252 len = a[0]*a[0] + a[1]*a[1]; \ 253 len = sqrt (len); \ 254 } 255 256 /* ========================================================== */ 257 /* Vector length */ 258 259 #define VEC_LENGTH(len,a) \ 260 { \ 261 len = (a)[0]*(a)[0] + (a)[1]*(a)[1]; \ 262 len += (a)[2]*(a)[2]; \ 263 len = sqrt (len); \ 264 } 265 266 /* ========================================================== */ 267 /* Vector length */ 268 269 #define VEC_LENGTH_4(len,a) \ 270 { \ 271 len = (a)[0]*(a)[0] + (a)[1]*(a)[1]; \ 272 len += (a)[2]*(a)[2]; \ 273 len += (a)[3] * (a)[3]; \ 274 len = sqrt (len); \ 275 } 276 277 /* ========================================================== */ 278 /* distance between two points */ 279 280 #define VEC_DISTANCE(len,va,vb) \ 281 { \ 282 gleDouble tmp[4]; \ 283 VEC_DIFF (tmp, vb, va); \ 284 VEC_LENGTH (len, tmp); \ 285 } 286 287 /* ========================================================== */ 288 /* Vector length */ 289 290 #define VEC_CONJUGATE_LENGTH(len,a) \ 291 { \ 292 len = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ 293 len = sqrt (len); \ 294 } 295 296 /* ========================================================== */ 297 /* Vector length */ 298 299 #define VEC_NORMALIZE(a) \ 300 { \ 301 double len; \ 302 VEC_LENGTH (len,a); \ 303 if (len != 0.0) { \ 304 len = 1.0 / len; \ 305 a[0] *= len; \ 306 a[1] *= len; \ 307 a[2] *= len; \ 308 } \ 309 } 310 311 /* ========================================================== */ 312 /* Vector length */ 313 314 #define VEC_RENORMALIZE(a,newlen) \ 315 { \ 316 double len; \ 317 VEC_LENGTH (len,a); \ 318 if (len != 0.0) { \ 319 len = newlen / len; \ 320 a[0] *= len; \ 321 a[1] *= len; \ 322 a[2] *= len; \ 323 } \ 324 } 325 326 /* ========================================================== */ 327 /* 3D Vector cross product yeilding vector */ 328 329 #define VEC_CROSS_PRODUCT(c,a,b) \ 330 { \ 331 c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ 332 c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ 333 c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ 334 } 335 336 /* ========================================================== */ 337 /* Vector perp -- assumes that n is of unit length 338 * accepts vector v, subtracts out any component parallel to n */ 339 340 #define VEC_PERP(vp,v,n) \ 341 { \ 342 double dot; \ 343 \ 344 VEC_DOT_PRODUCT (dot, v, n); \ 345 vp[0] = (v)[0] - (dot) * (n)[0]; \ 346 vp[1] = (v)[1] - (dot) * (n)[1]; \ 347 vp[2] = (v)[2] - (dot) * (n)[2]; \ 348 } 349 350 /* ========================================================== */ 351 /* Vector parallel -- assumes that n is of unit length 352 * accepts vector v, subtracts out any component perpendicular to n */ 353 354 #define VEC_PARALLEL(vp,v,n) \ 355 { \ 356 double dot; \ 357 \ 358 VEC_DOT_PRODUCT (dot, v, n); \ 359 vp[0] = (dot) * (n)[0]; \ 360 vp[1] = (dot) * (n)[1]; \ 361 vp[2] = (dot) * (n)[2]; \ 362 } 363 364 /* ========================================================== */ 365 /* Vector reflection -- assumes n is of unit length */ 366 /* Takes vector v, reflects it against reflector n, and returns vr */ 367 368 #define VEC_REFLECT(vr,v,n) \ 369 { \ 370 double dot; \ 371 \ 372 VEC_DOT_PRODUCT (dot, v, n); \ 373 vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ 374 vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ 375 vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ 376 } 377 378 /* ========================================================== */ 379 /* Vector blending */ 380 /* Takes two vectors a, b, blends them together */ 381 382 #define VEC_BLEND(vr,sa,a,sb,b) \ 383 { \ 384 \ 385 vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ 386 vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ 387 vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ 388 } 389 390 /* ========================================================== */ 391 /* Vector print */ 392 393 #define VEC_PRINT_2(a) \ 394 { \ 395 double len; \ 396 VEC_LENGTH_2 (len, a); \ 397 printf (" a is %f %f length of a is %f \n", a[0], a[1], len); \ 398 } 399 400 /* ========================================================== */ 401 /* Vector print */ 402 403 #define VEC_PRINT(a) \ 404 { \ 405 double len; \ 406 VEC_LENGTH (len, (a)); \ 407 printf (" a is %f %f %f length of a is %f \n", (a)[0], (a)[1], (a)[2], len); \ 408 } 409 410 /* ========================================================== */ 411 /* Vector print */ 412 413 #define VEC_PRINT_4(a) \ 414 { \ 415 double len; \ 416 VEC_LENGTH_4 (len, (a)); \ 417 printf (" a is %f %f %f %f length of a is %f \n", \ 418 (a)[0], (a)[1], (a)[2], (a)[3], len); \ 419 } 420 421 /* ========================================================== */ 422 /* print matrix */ 423 424 #define MAT_PRINT_4X4(mmm) { \ 425 int i,j; \ 426 printf ("matrix mmm is \n"); \ 427 if (mmm == NULL) { \ 428 printf (" Null \n"); \ 429 } else { \ 430 for (i=0; i<4; i++) { \ 431 for (j=0; j<4; j++) { \ 432 printf ("%f ", mmm[i][j]); \ 433 } \ 434 printf (" \n"); \ 435 } \ 436 } \ 437 } 438 439 /* ========================================================== */ 440 /* print matrix */ 441 442 #define MAT_PRINT_3X3(mmm) { \ 443 int i,j; \ 444 printf ("matrix mmm is \n"); \ 445 if (mmm == NULL) { \ 446 printf (" Null \n"); \ 447 } else { \ 448 for (i=0; i<3; i++) { \ 449 for (j=0; j<3; j++) { \ 450 printf ("%f ", mmm[i][j]); \ 451 } \ 452 printf (" \n"); \ 453 } \ 454 } \ 455 } 456 457 /* ========================================================== */ 458 /* print matrix */ 459 460 #define MAT_PRINT_2X3(mmm) { \ 461 int i,j; \ 462 printf ("matrix mmm is \n"); \ 463 if (mmm == NULL) { \ 464 printf (" Null \n"); \ 465 } else { \ 466 for (i=0; i<2; i++) { \ 467 for (j=0; j<3; j++) { \ 468 printf ("%f ", mmm[i][j]); \ 469 } \ 470 printf (" \n"); \ 471 } \ 472 } \ 473 } 474 475 /* ========================================================== */ 476 /* initialize matrix */ 477 478 #define IDENTIFY_MATRIX_3X3(m) \ 479 { \ 480 m[0][0] = 1.0; \ 481 m[0][1] = 0.0; \ 482 m[0][2] = 0.0; \ 483 \ 484 m[1][0] = 0.0; \ 485 m[1][1] = 1.0; \ 486 m[1][2] = 0.0; \ 487 \ 488 m[2][0] = 0.0; \ 489 m[2][1] = 0.0; \ 490 m[2][2] = 1.0; \ 491 } 492 493 /* ========================================================== */ 494 /* initialize matrix */ 495 496 #define IDENTIFY_MATRIX_4X4(m) \ 497 { \ 498 m[0][0] = 1.0; \ 499 m[0][1] = 0.0; \ 500 m[0][2] = 0.0; \ 501 m[0][3] = 0.0; \ 502 \ 503 m[1][0] = 0.0; \ 504 m[1][1] = 1.0; \ 505 m[1][2] = 0.0; \ 506 m[1][3] = 0.0; \ 507 \ 508 m[2][0] = 0.0; \ 509 m[2][1] = 0.0; \ 510 m[2][2] = 1.0; \ 511 m[2][3] = 0.0; \ 512 \ 513 m[3][0] = 0.0; \ 514 m[3][1] = 0.0; \ 515 m[3][2] = 0.0; \ 516 m[3][3] = 1.0; \ 517 } 518 519 /* ========================================================== */ 520 /* matrix copy */ 521 522 #define COPY_MATRIX_2X2(b,a) \ 523 { \ 524 b[0][0] = a[0][0]; \ 525 b[0][1] = a[0][1]; \ 526 \ 527 b[1][0] = a[1][0]; \ 528 b[1][1] = a[1][1]; \ 529 \ 530 } 531 532 /* ========================================================== */ 533 /* matrix copy */ 534 535 #define COPY_MATRIX_2X3(b,a) \ 536 { \ 537 b[0][0] = a[0][0]; \ 538 b[0][1] = a[0][1]; \ 539 b[0][2] = a[0][2]; \ 540 \ 541 b[1][0] = a[1][0]; \ 542 b[1][1] = a[1][1]; \ 543 b[1][2] = a[1][2]; \ 544 } 545 546 /* ========================================================== */ 547 /* matrix copy */ 548 549 #define COPY_MATRIX_3X3(b,a) \ 550 { \ 551 b[0][0] = a[0][0]; \ 552 b[0][1] = a[0][1]; \ 553 b[0][2] = a[0][2]; \ 554 \ 555 b[1][0] = a[1][0]; \ 556 b[1][1] = a[1][1]; \ 557 b[1][2] = a[1][2]; \ 558 \ 559 b[2][0] = a[2][0]; \ 560 b[2][1] = a[2][1]; \ 561 b[2][2] = a[2][2]; \ 562 } 563 564 /* ========================================================== */ 565 /* matrix copy */ 566 567 #define COPY_MATRIX_4X4(b,a) \ 568 { \ 569 b[0][0] = a[0][0]; \ 570 b[0][1] = a[0][1]; \ 571 b[0][2] = a[0][2]; \ 572 b[0][3] = a[0][3]; \ 573 \ 574 b[1][0] = a[1][0]; \ 575 b[1][1] = a[1][1]; \ 576 b[1][2] = a[1][2]; \ 577 b[1][3] = a[1][3]; \ 578 \ 579 b[2][0] = a[2][0]; \ 580 b[2][1] = a[2][1]; \ 581 b[2][2] = a[2][2]; \ 582 b[2][3] = a[2][3]; \ 583 \ 584 b[3][0] = a[3][0]; \ 585 b[3][1] = a[3][1]; \ 586 b[3][2] = a[3][2]; \ 587 b[3][3] = a[3][3]; \ 588 } 589 590 /* ========================================================== */ 591 /* matrix transpose */ 592 593 #define TRANSPOSE_MATRIX_2X2(b,a) \ 594 { \ 595 b[0][0] = a[0][0]; \ 596 b[0][1] = a[1][0]; \ 597 \ 598 b[1][0] = a[0][1]; \ 599 b[1][1] = a[1][1]; \ 600 } 601 602 /* ========================================================== */ 603 /* matrix transpose */ 604 605 #define TRANSPOSE_MATRIX_3X3(b,a) \ 606 { \ 607 b[0][0] = a[0][0]; \ 608 b[0][1] = a[1][0]; \ 609 b[0][2] = a[2][0]; \ 610 \ 611 b[1][0] = a[0][1]; \ 612 b[1][1] = a[1][1]; \ 613 b[1][2] = a[2][1]; \ 614 \ 615 b[2][0] = a[0][2]; \ 616 b[2][1] = a[1][2]; \ 617 b[2][2] = a[2][2]; \ 618 } 619 620 /* ========================================================== */ 621 /* matrix transpose */ 622 623 #define TRANSPOSE_MATRIX_4X4(b,a) \ 624 { \ 625 b[0][0] = a[0][0]; \ 626 b[0][1] = a[1][0]; \ 627 b[0][2] = a[2][0]; \ 628 b[0][3] = a[3][0]; \ 629 \ 630 b[1][0] = a[0][1]; \ 631 b[1][1] = a[1][1]; \ 632 b[1][2] = a[2][1]; \ 633 b[1][3] = a[3][1]; \ 634 \ 635 b[2][0] = a[0][2]; \ 636 b[2][1] = a[1][2]; \ 637 b[2][2] = a[2][2]; \ 638 b[2][3] = a[3][2]; \ 639 \ 640 b[3][0] = a[0][3]; \ 641 b[3][1] = a[1][3]; \ 642 b[3][2] = a[2][3]; \ 643 b[3][3] = a[3][3]; \ 644 } 645 646 /* ========================================================== */ 647 /* multiply matrix by scalar */ 648 649 #define SCALE_MATRIX_2X2(b,s,a) \ 650 { \ 651 b[0][0] = (s) * a[0][0]; \ 652 b[0][1] = (s) * a[0][1]; \ 653 \ 654 b[1][0] = (s) * a[1][0]; \ 655 b[1][1] = (s) * a[1][1]; \ 656 } 657 658 /* ========================================================== */ 659 /* multiply matrix by scalar */ 660 661 #define SCALE_MATRIX_3X3(b,s,a) \ 662 { \ 663 b[0][0] = (s) * a[0][0]; \ 664 b[0][1] = (s) * a[0][1]; \ 665 b[0][2] = (s) * a[0][2]; \ 666 \ 667 b[1][0] = (s) * a[1][0]; \ 668 b[1][1] = (s) * a[1][1]; \ 669 b[1][2] = (s) * a[1][2]; \ 670 \ 671 b[2][0] = (s) * a[2][0]; \ 672 b[2][1] = (s) * a[2][1]; \ 673 b[2][2] = (s) * a[2][2]; \ 674 } 675 676 /* ========================================================== */ 677 /* multiply matrix by scalar */ 678 679 #define SCALE_MATRIX_4X4(b,s,a) \ 680 { \ 681 b[0][0] = (s) * a[0][0]; \ 682 b[0][1] = (s) * a[0][1]; \ 683 b[0][2] = (s) * a[0][2]; \ 684 b[0][3] = (s) * a[0][3]; \ 685 \ 686 b[1][0] = (s) * a[1][0]; \ 687 b[1][1] = (s) * a[1][1]; \ 688 b[1][2] = (s) * a[1][2]; \ 689 b[1][3] = (s) * a[1][3]; \ 690 \ 691 b[2][0] = (s) * a[2][0]; \ 692 b[2][1] = (s) * a[2][1]; \ 693 b[2][2] = (s) * a[2][2]; \ 694 b[2][3] = (s) * a[2][3]; \ 695 \ 696 b[3][0] = s * a[3][0]; \ 697 b[3][1] = s * a[3][1]; \ 698 b[3][2] = s * a[3][2]; \ 699 b[3][3] = s * a[3][3]; \ 700 } 701 702 /* ========================================================== */ 703 /* multiply matrix by scalar */ 704 705 #define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ 706 { \ 707 b[0][0] += (s) * a[0][0]; \ 708 b[0][1] += (s) * a[0][1]; \ 709 \ 710 b[1][0] += (s) * a[1][0]; \ 711 b[1][1] += (s) * a[1][1]; \ 712 } 713 714 /* +========================================================== */ 715 /* multiply matrix by scalar */ 716 717 #define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ 718 { \ 719 b[0][0] += (s) * a[0][0]; \ 720 b[0][1] += (s) * a[0][1]; \ 721 b[0][2] += (s) * a[0][2]; \ 722 \ 723 b[1][0] += (s) * a[1][0]; \ 724 b[1][1] += (s) * a[1][1]; \ 725 b[1][2] += (s) * a[1][2]; \ 726 \ 727 b[2][0] += (s) * a[2][0]; \ 728 b[2][1] += (s) * a[2][1]; \ 729 b[2][2] += (s) * a[2][2]; \ 730 } 731 732 /* +========================================================== */ 733 /* multiply matrix by scalar */ 734 735 #define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ 736 { \ 737 b[0][0] += (s) * a[0][0]; \ 738 b[0][1] += (s) * a[0][1]; \ 739 b[0][2] += (s) * a[0][2]; \ 740 b[0][3] += (s) * a[0][3]; \ 741 \ 742 b[1][0] += (s) * a[1][0]; \ 743 b[1][1] += (s) * a[1][1]; \ 744 b[1][2] += (s) * a[1][2]; \ 745 b[1][3] += (s) * a[1][3]; \ 746 \ 747 b[2][0] += (s) * a[2][0]; \ 748 b[2][1] += (s) * a[2][1]; \ 749 b[2][2] += (s) * a[2][2]; \ 750 b[2][3] += (s) * a[2][3]; \ 751 \ 752 b[3][0] += (s) * a[3][0]; \ 753 b[3][1] += (s) * a[3][1]; \ 754 b[3][2] += (s) * a[3][2]; \ 755 b[3][3] += (s) * a[3][3]; \ 756 } 757 758 /* +========================================================== */ 759 /* matrix product */ 760 /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ 761 762 #define MATRIX_PRODUCT_2X2(c,a,b) \ 763 { \ 764 c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ 765 c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ 766 \ 767 c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ 768 c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ 769 \ 770 } 771 772 /* ========================================================== */ 773 /* matrix product */ 774 /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ 775 776 #define MATRIX_PRODUCT_3X3(c,a,b) \ 777 { \ 778 c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ 779 c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ 780 c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ 781 \ 782 c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ 783 c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ 784 c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ 785 \ 786 c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ 787 c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ 788 c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ 789 } 790 791 /* ========================================================== */ 792 /* matrix product */ 793 /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ 794 795 #define MATRIX_PRODUCT_4X4(c,a,b) \ 796 { \ 797 c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\ 798 c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\ 799 c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\ 800 c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\ 801 \ 802 c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\ 803 c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\ 804 c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\ 805 c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\ 806 \ 807 c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\ 808 c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\ 809 c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\ 810 c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\ 811 \ 812 c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\ 813 c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\ 814 c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\ 815 c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\ 816 } 817 818 /* ========================================================== */ 819 /* matrix times vector */ 820 821 #define MAT_DOT_VEC_2X2(p,m,v) \ 822 { \ 823 p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ 824 p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ 825 } 826 827 /* ========================================================== */ 828 /* matrix times vector */ 829 830 #define MAT_DOT_VEC_3X3(p,m,v) \ 831 { \ 832 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ 833 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ 834 p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ 835 } 836 837 /* ========================================================== */ 838 /* matrix times vector */ 839 840 #define MAT_DOT_VEC_4X4(p,m,v) \ 841 { \ 842 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ 843 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ 844 p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ 845 p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ 846 } 847 848 /* ========================================================== */ 849 /* vector transpose times matrix */ 850 /* p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ 851 852 #define VEC_DOT_MAT_3X3(p,v,m) \ 853 { \ 854 p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ 855 p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ 856 p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ 857 } 858 859 /* ========================================================== */ 860 /* affine matrix times vector */ 861 /* The matrix is assumed to be an affine matrix, with last two 862 * entries representing a translation */ 863 864 #define MAT_DOT_VEC_2X3(p,m,v) \ 865 { \ 866 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ 867 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ 868 } 869 870 /* ========================================================== */ 871 /* inverse transpose of matrix times vector 872 * 873 * This macro computes inverse transpose of matrix m, 874 * and multiplies vector v into it, to yeild vector p 875 * 876 * DANGER !!! Do Not use this on normal vectors!!! 877 * It will leave normals the wrong length !!! 878 * See macro below for use on normals. 879 */ 880 881 #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ 882 { \ 883 gleDouble det; \ 884 \ 885 det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ 886 p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ 887 p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ 888 \ 889 /* if matrix not singular, and not orthonormal, then renormalize */ \ 890 if ((det!=1.0) && (det != 0.0)) { \ 891 det = 1.0 / det; \ 892 p[0] *= det; \ 893 p[1] *= det; \ 894 } \ 895 } 896 897 /* ========================================================== */ 898 /* transform normal vector by inverse transpose of matrix 899 * and then renormalize the vector 900 * 901 * This macro computes inverse transpose of matrix m, 902 * and multiplies vector v into it, to yeild vector p 903 * Vector p is then normalized. 904 */ 905 906 907 #define NORM_XFORM_2X2(p,m,v) \ 908 { \ 909 double len; \ 910 \ 911 /* do nothing if off-diagonals are zero and diagonals are \ 912 * equal */ \ 913 if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ 914 p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ 915 p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ 916 \ 917 len = p[0]*p[0] + p[1]*p[1]; \ 918 len = 1.0 / sqrt (len); \ 919 p[0] *= len; \ 920 p[1] *= len; \ 921 } else { \ 922 VEC_COPY_2 (p, v); \ 923 } \ 924 } 925 926 /* ========================================================== */ 927 /* outer product of vector times vector transpose 928 * 929 * The outer product of vector v and vector transpose t yeilds 930 * dyadic matrix m. 931 */ 932 933 #define OUTER_PRODUCT_2X2(m,v,t) \ 934 { \ 935 m[0][0] = v[0] * t[0]; \ 936 m[0][1] = v[0] * t[1]; \ 937 \ 938 m[1][0] = v[1] * t[0]; \ 939 m[1][1] = v[1] * t[1]; \ 940 } 941 942 /* ========================================================== */ 943 /* outer product of vector times vector transpose 944 * 945 * The outer product of vector v and vector transpose t yeilds 946 * dyadic matrix m. 947 */ 948 949 #define OUTER_PRODUCT_3X3(m,v,t) \ 950 { \ 951 m[0][0] = v[0] * t[0]; \ 952 m[0][1] = v[0] * t[1]; \ 953 m[0][2] = v[0] * t[2]; \ 954 \ 955 m[1][0] = v[1] * t[0]; \ 956 m[1][1] = v[1] * t[1]; \ 957 m[1][2] = v[1] * t[2]; \ 958 \ 959 m[2][0] = v[2] * t[0]; \ 960 m[2][1] = v[2] * t[1]; \ 961 m[2][2] = v[2] * t[2]; \ 962 } 963 964 /* ========================================================== */ 965 /* outer product of vector times vector transpose 966 * 967 * The outer product of vector v and vector transpose t yeilds 968 * dyadic matrix m. 969 */ 970 971 #define OUTER_PRODUCT_4X4(m,v,t) \ 972 { \ 973 m[0][0] = v[0] * t[0]; \ 974 m[0][1] = v[0] * t[1]; \ 975 m[0][2] = v[0] * t[2]; \ 976 m[0][3] = v[0] * t[3]; \ 977 \ 978 m[1][0] = v[1] * t[0]; \ 979 m[1][1] = v[1] * t[1]; \ 980 m[1][2] = v[1] * t[2]; \ 981 m[1][3] = v[1] * t[3]; \ 982 \ 983 m[2][0] = v[2] * t[0]; \ 984 m[2][1] = v[2] * t[1]; \ 985 m[2][2] = v[2] * t[2]; \ 986 m[2][3] = v[2] * t[3]; \ 987 \ 988 m[3][0] = v[3] * t[0]; \ 989 m[3][1] = v[3] * t[1]; \ 990 m[3][2] = v[3] * t[2]; \ 991 m[3][3] = v[3] * t[3]; \ 992 } 993 994 /* +========================================================== */ 995 /* outer product of vector times vector transpose 996 * 997 * The outer product of vector v and vector transpose t yeilds 998 * dyadic matrix m. 999 */ 1000 1001 #define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ 1002 { \ 1003 m[0][0] += v[0] * t[0]; \ 1004 m[0][1] += v[0] * t[1]; \ 1005 \ 1006 m[1][0] += v[1] * t[0]; \ 1007 m[1][1] += v[1] * t[1]; \ 1008 } 1009 1010 /* +========================================================== */ 1011 /* outer product of vector times vector transpose 1012 * 1013 * The outer product of vector v and vector transpose t yeilds 1014 * dyadic matrix m. 1015 */ 1016 1017 #define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ 1018 { \ 1019 m[0][0] += v[0] * t[0]; \ 1020 m[0][1] += v[0] * t[1]; \ 1021 m[0][2] += v[0] * t[2]; \ 1022 \ 1023 m[1][0] += v[1] * t[0]; \ 1024 m[1][1] += v[1] * t[1]; \ 1025 m[1][2] += v[1] * t[2]; \ 1026 \ 1027 m[2][0] += v[2] * t[0]; \ 1028 m[2][1] += v[2] * t[1]; \ 1029 m[2][2] += v[2] * t[2]; \ 1030 } 1031 1032 /* +========================================================== */ 1033 /* outer product of vector times vector transpose 1034 * 1035 * The outer product of vector v and vector transpose t yeilds 1036 * dyadic matrix m. 1037 */ 1038 1039 #define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ 1040 { \ 1041 m[0][0] += v[0] * t[0]; \ 1042 m[0][1] += v[0] * t[1]; \ 1043 m[0][2] += v[0] * t[2]; \ 1044 m[0][3] += v[0] * t[3]; \ 1045 \ 1046 m[1][0] += v[1] * t[0]; \ 1047 m[1][1] += v[1] * t[1]; \ 1048 m[1][2] += v[1] * t[2]; \ 1049 m[1][3] += v[1] * t[3]; \ 1050 \ 1051 m[2][0] += v[2] * t[0]; \ 1052 m[2][1] += v[2] * t[1]; \ 1053 m[2][2] += v[2] * t[2]; \ 1054 m[2][3] += v[2] * t[3]; \ 1055 \ 1056 m[3][0] += v[3] * t[0]; \ 1057 m[3][1] += v[3] * t[1]; \ 1058 m[3][2] += v[3] * t[2]; \ 1059 m[3][3] += v[3] * t[3]; \ 1060 } 1061 1062 /* +========================================================== */ 1063 /* determinant of matrix 1064 * 1065 * Computes determinant of matrix m, returning d 1066 */ 1067 1068 #define DETERMINANT_2X2(d,m) \ 1069 { \ 1070 d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ 1071 } 1072 1073 /* ========================================================== */ 1074 /* determinant of matrix 1075 * 1076 * Computes determinant of matrix m, returning d 1077 */ 1078 1079 #define DETERMINANT_3X3(d,m) \ 1080 { \ 1081 d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ 1082 d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ 1083 d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ 1084 } 1085 1086 /* ========================================================== */ 1087 /* i,j,th cofactor of a 4x4 matrix 1088 * 1089 */ 1090 1091 #define COFACTOR_4X4_IJ(fac,m,i,j) \ 1092 { \ 1093 int ii[4], jj[4], k; \ 1094 \ 1095 /* compute which row, columnt to skip */ \ 1096 for (k=0; k<i; k++) ii[k] = k; \ 1097 for (k=i; k<3; k++) ii[k] = k+1; \ 1098 for (k=0; k<j; k++) jj[k] = k; \ 1099 for (k=j; k<3; k++) jj[k] = k+1; \ 1100 \ 1101 (fac) = m[ii[0]][jj[0]] * (m[ii[1]][jj[1]]*m[ii[2]][jj[2]] \ 1102 - m[ii[1]][jj[2]]*m[ii[2]][jj[1]]); \ 1103 (fac) -= m[ii[0]][jj[1]] * (m[ii[1]][jj[0]]*m[ii[2]][jj[2]] \ 1104 - m[ii[1]][jj[2]]*m[ii[2]][jj[0]]);\ 1105 (fac) += m[ii[0]][jj[2]] * (m[ii[1]][jj[0]]*m[ii[2]][jj[1]] \ 1106 - m[ii[1]][jj[1]]*m[ii[2]][jj[0]]);\ 1107 \ 1108 /* compute sign */ \ 1109 k = i+j; \ 1110 if ( k != (k/2)*2) { \ 1111 (fac) = -(fac); \ 1112 } \ 1113 } 1114 1115 /* ========================================================== */ 1116 /* determinant of matrix 1117 * 1118 * Computes determinant of matrix m, returning d 1119 */ 1120 1121 #define DETERMINANT_4X4(d,m) \ 1122 { \ 1123 double cofac; \ 1124 COFACTOR_4X4_IJ (cofac, m, 0, 0); \ 1125 d = m[0][0] * cofac; \ 1126 COFACTOR_4X4_IJ (cofac, m, 0, 1); \ 1127 d += m[0][1] * cofac; \ 1128 COFACTOR_4X4_IJ (cofac, m, 0, 2); \ 1129 d += m[0][2] * cofac; \ 1130 COFACTOR_4X4_IJ (cofac, m, 0, 3); \ 1131 d += m[0][3] * cofac; \ 1132 } 1133 1134 /* ========================================================== */ 1135 /* cofactor of matrix 1136 * 1137 * Computes cofactor of matrix m, returning a 1138 */ 1139 1140 #define COFACTOR_2X2(a,m) \ 1141 { \ 1142 a[0][0] = (m)[1][1]; \ 1143 a[0][1] = - (m)[1][0]; \ 1144 a[1][0] = - (m)[0][1]; \ 1145 a[1][1] = (m)[0][0]; \ 1146 } 1147 1148 /* ========================================================== */ 1149 /* cofactor of matrix 1150 * 1151 * Computes cofactor of matrix m, returning a 1152 */ 1153 1154 #define COFACTOR_3X3(a,m) \ 1155 { \ 1156 a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ 1157 a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ 1158 a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ 1159 a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ 1160 a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ 1161 a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ 1162 a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ 1163 a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ 1164 a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ 1165 } 1166 1167 /* ========================================================== */ 1168 /* cofactor of matrix 1169 * 1170 * Computes cofactor of matrix m, returning a 1171 */ 1172 1173 #define COFACTOR_4X4(a,m) \ 1174 { \ 1175 int i,j; \ 1176 \ 1177 for (i=0; i<4; i++) { \ 1178 for (j=0; j<4; j++) { \ 1179 COFACTOR_4X4_IJ (a[i][j], m, i, j); \ 1180 } \ 1181 } \ 1182 } 1183 1184 /* ========================================================== */ 1185 /* adjoint of matrix 1186 * 1187 * Computes adjoint of matrix m, returning a 1188 * (Note that adjoint is just the transpose of the cofactor matrix) 1189 */ 1190 1191 #define ADJOINT_2X2(a,m) \ 1192 { \ 1193 a[0][0] = (m)[1][1]; \ 1194 a[1][0] = - (m)[1][0]; \ 1195 a[0][1] = - (m)[0][1]; \ 1196 a[1][1] = (m)[0][0]; \ 1197 } 1198 1199 /* ========================================================== */ 1200 /* adjoint of matrix 1201 * 1202 * Computes adjoint of matrix m, returning a 1203 * (Note that adjoint is just the transpose of the cofactor matrix) 1204 */ 1205 1206 1207 #define ADJOINT_3X3(a,m) \ 1208 { \ 1209 a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ 1210 a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ 1211 a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ 1212 a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ 1213 a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ 1214 a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ 1215 a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ 1216 a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ 1217 a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ 1218 } 1219 1220 /* ========================================================== */ 1221 /* adjoint of matrix 1222 * 1223 * Computes adjoint of matrix m, returning a 1224 * (Note that adjoint is just the transpose of the cofactor matrix) 1225 */ 1226 1227 #define ADJOINT_4X4(a,m) \ 1228 { \ 1229 int i,j; \ 1230 \ 1231 for (i=0; i<4; i++) { \ 1232 for (j=0; j<4; j++) { \ 1233 COFACTOR_4X4_IJ (a[j][i], m, i, j); \ 1234 } \ 1235 } \ 1236 } 1237 1238 /* ========================================================== */ 1239 /* compute adjoint of matrix and scale 1240 * 1241 * Computes adjoint of matrix m, scales it by s, returning a 1242 */ 1243 1244 #define SCALE_ADJOINT_2X2(a,s,m) \ 1245 { \ 1246 a[0][0] = (s) * m[1][1]; \ 1247 a[1][0] = - (s) * m[1][0]; \ 1248 a[0][1] = - (s) * m[0][1]; \ 1249 a[1][1] = (s) * m[0][0]; \ 1250 } 1251 1252 /* ========================================================== */ 1253 /* compute adjoint of matrix and scale 1254 * 1255 * Computes adjoint of matrix m, scales it by s, returning a 1256 */ 1257 1258 #define SCALE_ADJOINT_3X3(a,s,m) \ 1259 { \ 1260 a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ 1261 a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ 1262 a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ 1263 \ 1264 a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ 1265 a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ 1266 a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ 1267 \ 1268 a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ 1269 a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ 1270 a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ 1271 } 1272 1273 /* ========================================================== */ 1274 /* compute adjoint of matrix and scale 1275 * 1276 * Computes adjoint of matrix m, scales it by s, returning a 1277 */ 1278 1279 #define SCALE_ADJOINT_4X4(a,s,m) \ 1280 { \ 1281 int i,j; \ 1282 \ 1283 for (i=0; i<4; i++) { \ 1284 for (j=0; j<4; j++) { \ 1285 COFACTOR_4X4_IJ (a[j][i], m, i, j); \ 1286 a[j][i] *= s; \ 1287 } \ 1288 } \ 1289 } 1290 1291 /* ========================================================== */ 1292 /* inverse of matrix 1293 * 1294 * Compute inverse of matrix a, returning determinant m and 1295 * inverse b 1296 */ 1297 1298 #define INVERT_2X2(b,det,a) \ 1299 { \ 1300 double tmp; \ 1301 DETERMINANT_2X2 (det, a); \ 1302 tmp = 1.0 / (det); \ 1303 SCALE_ADJOINT_2X2 (b, tmp, a); \ 1304 } 1305 1306 /* ========================================================== */ 1307 /* inverse of matrix 1308 * 1309 * Compute inverse of matrix a, returning determinant m and 1310 * inverse b 1311 */ 1312 1313 #define INVERT_3X3(b,det,a) \ 1314 { \ 1315 double tmp; \ 1316 DETERMINANT_3X3 (det, a); \ 1317 tmp = 1.0 / (det); \ 1318 SCALE_ADJOINT_3X3 (b, tmp, a); \ 1319 } 1320 1321 /* ========================================================== */ 1322 /* inverse of matrix 1323 * 1324 * Compute inverse of matrix a, returning determinant m and 1325 * inverse b 1326 */ 1327 1328 #define INVERT_4X4(b,det,a) \ 1329 { \ 1330 double tmp; \ 1331 DETERMINANT_4X4 (det, a); \ 1332 tmp = 1.0 / (det); \ 1333 SCALE_ADJOINT_4X4 (b, tmp, a); \ 1334 } 1335 1336 /* ========================================================== */ 1337 #if defined(__cplusplus) || defined(c_plusplus) 1338 } 1339 #endif 1340 1341 #endif /* __GUTIL_VECTOR_H__ */ 1342 /* ===================== END OF FILE ======================== */