1 #ifndef GIM_VECTOR_H_INCLUDED 2 #define GIM_VECTOR_H_INCLUDED 3 4 /*! \file gim_geometry.h 5 \author Francisco Le�n 6 */ 7 /* 8 ----------------------------------------------------------------------------- 9 This source file is part of GIMPACT Library. 10 11 For the latest info, see http://gimpact.sourceforge.net/ 12 13 Copyright (c) 2006 Francisco Leon. C.C. 80087371. 14 email: projectileman@yahoo.com 15 16 This library is free software; you can redistribute it and/or 17 modify it under the terms of EITHER: 18 (1) The GNU Lesser General Public License as published by the Free 19 Software Foundation; either version 2.1 of the License, or (at 20 your option) any later version. The text of the GNU Lesser 21 General Public License is included with this library in the 22 file GIMPACT-LICENSE-LGPL.TXT. 23 (2) The BSD-style license that is included with this library in 24 the file GIMPACT-LICENSE-BSD.TXT. 25 26 This library is distributed in the hope that it will be useful, 27 but WITHOUT ANY WARRANTY; without even the implied warranty of 28 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 29 GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details. 30 31 ----------------------------------------------------------------------------- 32 */ 33 34 35 #include "GIMPACT/gim_math.h" 36 37 /*! \defgroup GEOMETRIC_TYPES 38 \brief 39 Basic types and constants for geometry 40 */ 41 //! @{ 42 43 //! Integer vector 2D 44 typedef GINT32 vec2i[2]; 45 //! Integer vector 3D 46 typedef GINT32 vec3i[3]; 47 //! Integer vector 4D 48 typedef GINT32 vec4i[4]; 49 50 //! Float vector 2D 51 typedef GREAL vec2f[2]; 52 //! Float vector 3D 53 typedef GREAL vec3f[3]; 54 //! Float vector 4D 55 typedef GREAL vec4f[4]; 56 57 //! Matrix 2D, row ordered 58 typedef GREAL mat2f[2][2]; 59 //! Matrix 3D, row ordered 60 typedef GREAL mat3f[3][3]; 61 //! Matrix 4D, row ordered 62 typedef GREAL mat4f[4][4]; 63 64 //! Quaternion 65 typedef GREAL quatf[4]; 66 67 //! Axis aligned box 68 struct aabb3f{ 69 GREAL minX; 70 GREAL maxX; 71 GREAL minY; 72 GREAL maxY; 73 GREAL minZ; 74 GREAL maxZ; 75 }; 76 //typedef struct _aabb3f aabb3f; 77 //! @} 78 79 80 /*! \defgroup VECTOR_OPERATIONS 81 Operations for vectors : vec2f,vec3f and vec4f 82 */ 83 //! @{ 84 85 //! Zero out a 2D vector 86 #define VEC_ZERO_2(a) \ 87 { \ 88 (a)[0] = (a)[1] = 0.0f; \ 89 }\ 90 91 92 //! Zero out a 3D vector 93 #define VEC_ZERO(a) \ 94 { \ 95 (a)[0] = (a)[1] = (a)[2] = 0.0f; \ 96 }\ 97 98 99 /// Zero out a 4D vector 100 #define VEC_ZERO_4(a) \ 101 { \ 102 (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ 103 }\ 104 105 106 /// Vector copy 107 #define VEC_COPY_2(b,a) \ 108 { \ 109 (b)[0] = (a)[0]; \ 110 (b)[1] = (a)[1]; \ 111 }\ 112 113 114 /// Copy 3D vector 115 #define VEC_COPY(b,a) \ 116 { \ 117 (b)[0] = (a)[0]; \ 118 (b)[1] = (a)[1]; \ 119 (b)[2] = (a)[2]; \ 120 }\ 121 122 123 /// Copy 4D vector 124 #define VEC_COPY_4(b,a) \ 125 { \ 126 (b)[0] = (a)[0]; \ 127 (b)[1] = (a)[1]; \ 128 (b)[2] = (a)[2]; \ 129 (b)[3] = (a)[3]; \ 130 }\ 131 132 133 /// Vector difference 134 #define VEC_DIFF_2(v21,v2,v1) \ 135 { \ 136 (v21)[0] = (v2)[0] - (v1)[0]; \ 137 (v21)[1] = (v2)[1] - (v1)[1]; \ 138 }\ 139 140 141 /// Vector difference 142 #define VEC_DIFF(v21,v2,v1) \ 143 { \ 144 (v21)[0] = (v2)[0] - (v1)[0]; \ 145 (v21)[1] = (v2)[1] - (v1)[1]; \ 146 (v21)[2] = (v2)[2] - (v1)[2]; \ 147 }\ 148 149 150 /// Vector difference 151 #define VEC_DIFF_4(v21,v2,v1) \ 152 { \ 153 (v21)[0] = (v2)[0] - (v1)[0]; \ 154 (v21)[1] = (v2)[1] - (v1)[1]; \ 155 (v21)[2] = (v2)[2] - (v1)[2]; \ 156 (v21)[3] = (v2)[3] - (v1)[3]; \ 157 }\ 158 159 160 /// Vector sum 161 #define VEC_SUM_2(v21,v2,v1) \ 162 { \ 163 (v21)[0] = (v2)[0] + (v1)[0]; \ 164 (v21)[1] = (v2)[1] + (v1)[1]; \ 165 }\ 166 167 168 /// Vector sum 169 #define VEC_SUM(v21,v2,v1) \ 170 { \ 171 (v21)[0] = (v2)[0] + (v1)[0]; \ 172 (v21)[1] = (v2)[1] + (v1)[1]; \ 173 (v21)[2] = (v2)[2] + (v1)[2]; \ 174 }\ 175 176 177 /// Vector sum 178 #define VEC_SUM_4(v21,v2,v1) \ 179 { \ 180 (v21)[0] = (v2)[0] + (v1)[0]; \ 181 (v21)[1] = (v2)[1] + (v1)[1]; \ 182 (v21)[2] = (v2)[2] + (v1)[2]; \ 183 (v21)[3] = (v2)[3] + (v1)[3]; \ 184 }\ 185 186 187 /// scalar times vector 188 #define VEC_SCALE_2(c,a,b) \ 189 { \ 190 (c)[0] = (a)*(b)[0]; \ 191 (c)[1] = (a)*(b)[1]; \ 192 }\ 193 194 195 /// scalar times vector 196 #define VEC_SCALE(c,a,b) \ 197 { \ 198 (c)[0] = (a)*(b)[0]; \ 199 (c)[1] = (a)*(b)[1]; \ 200 (c)[2] = (a)*(b)[2]; \ 201 }\ 202 203 204 /// scalar times vector 205 #define VEC_SCALE_4(c,a,b) \ 206 { \ 207 (c)[0] = (a)*(b)[0]; \ 208 (c)[1] = (a)*(b)[1]; \ 209 (c)[2] = (a)*(b)[2]; \ 210 (c)[3] = (a)*(b)[3]; \ 211 }\ 212 213 214 /// accumulate scaled vector 215 #define VEC_ACCUM_2(c,a,b) \ 216 { \ 217 (c)[0] += (a)*(b)[0]; \ 218 (c)[1] += (a)*(b)[1]; \ 219 }\ 220 221 222 /// accumulate scaled vector 223 #define VEC_ACCUM(c,a,b) \ 224 { \ 225 (c)[0] += (a)*(b)[0]; \ 226 (c)[1] += (a)*(b)[1]; \ 227 (c)[2] += (a)*(b)[2]; \ 228 }\ 229 230 231 /// accumulate scaled vector 232 #define VEC_ACCUM_4(c,a,b) \ 233 { \ 234 (c)[0] += (a)*(b)[0]; \ 235 (c)[1] += (a)*(b)[1]; \ 236 (c)[2] += (a)*(b)[2]; \ 237 (c)[3] += (a)*(b)[3]; \ 238 }\ 239 240 241 /// Vector dot product 242 #define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1]) 243 244 245 /// Vector dot product 246 #define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) 247 248 /// Vector dot product 249 #define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3]) 250 251 /// vector impact parameter (squared) 252 #define VEC_IMPACT_SQ(bsq,direction,position) {\ 253 GREAL _llel_ = VEC_DOT(direction, position);\ 254 bsq = VEC_DOT(position, position) - _llel_*_llel_;\ 255 }\ 256 257 258 /// vector impact parameter 259 #define VEC_IMPACT(bsq,direction,position) {\ 260 VEC_IMPACT_SQ(bsq,direction,position); \ 261 GIM_SQRT(bsq,bsq); \ 262 }\ 263 264 /// Vector length 265 #define VEC_LENGTH_2(a,l)\ 266 {\ 267 GREAL _pp = VEC_DOT_2(a,a);\ 268 GIM_SQRT(_pp,l);\ 269 }\ 270 271 272 /// Vector length 273 #define VEC_LENGTH(a,l)\ 274 {\ 275 GREAL _pp = VEC_DOT(a,a);\ 276 GIM_SQRT(_pp,l);\ 277 }\ 278 279 280 /// Vector length 281 #define VEC_LENGTH_4(a,l)\ 282 {\ 283 GREAL _pp = VEC_DOT_4(a,a);\ 284 GIM_SQRT(_pp,l);\ 285 }\ 286 287 /// Vector inv length 288 #define VEC_INV_LENGTH_2(a,l)\ 289 {\ 290 GREAL _pp = VEC_DOT_2(a,a);\ 291 GIM_INV_SQRT(_pp,l);\ 292 }\ 293 294 295 /// Vector inv length 296 #define VEC_INV_LENGTH(a,l)\ 297 {\ 298 GREAL _pp = VEC_DOT(a,a);\ 299 GIM_INV_SQRT(_pp,l);\ 300 }\ 301 302 303 /// Vector inv length 304 #define VEC_INV_LENGTH_4(a,l)\ 305 {\ 306 GREAL _pp = VEC_DOT_4(a,a);\ 307 GIM_INV_SQRT(_pp,l);\ 308 }\ 309 310 311 312 /// distance between two points 313 #define VEC_DISTANCE(_len,_va,_vb) {\ 314 vec3f _tmp_; \ 315 VEC_DIFF(_tmp_, _vb, _va); \ 316 VEC_LENGTH(_tmp_,_len); \ 317 }\ 318 319 320 /// Vector length 321 #define VEC_CONJUGATE_LENGTH(a,l)\ 322 {\ 323 GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ 324 GIM_SQRT(_pp,l);\ 325 }\ 326 327 328 /// Vector length 329 #define VEC_NORMALIZE(a) { \ 330 GREAL len;\ 331 VEC_INV_LENGTH(a,len); \ 332 if(len<G_REAL_INFINITY)\ 333 {\ 334 a[0] *= len; \ 335 a[1] *= len; \ 336 a[2] *= len; \ 337 } \ 338 }\ 339 340 /// Set Vector size 341 #define VEC_RENORMALIZE(a,newlen) { \ 342 GREAL len;\ 343 VEC_INV_LENGTH(a,len); \ 344 if(len<G_REAL_INFINITY)\ 345 {\ 346 len *= newlen;\ 347 a[0] *= len; \ 348 a[1] *= len; \ 349 a[2] *= len; \ 350 } \ 351 }\ 352 353 /// Vector cross 354 #define VEC_CROSS(c,a,b) \ 355 { \ 356 c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ 357 c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ 358 c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ 359 }\ 360 361 362 /*! Vector perp -- assumes that n is of unit length 363 * accepts vector v, subtracts out any component parallel to n */ 364 #define VEC_PERPENDICULAR(vp,v,n) \ 365 { \ 366 GREAL dot = VEC_DOT(v, n); \ 367 vp[0] = (v)[0] - dot*(n)[0]; \ 368 vp[1] = (v)[1] - dot*(n)[1]; \ 369 vp[2] = (v)[2] - dot*(n)[2]; \ 370 }\ 371 372 373 /*! Vector parallel -- assumes that n is of unit length */ 374 #define VEC_PARALLEL(vp,v,n) \ 375 { \ 376 GREAL dot = VEC_DOT(v, n); \ 377 vp[0] = (dot) * (n)[0]; \ 378 vp[1] = (dot) * (n)[1]; \ 379 vp[2] = (dot) * (n)[2]; \ 380 }\ 381 382 /*! Same as Vector parallel -- n can have any length 383 * accepts vector v, subtracts out any component perpendicular to n */ 384 #define VEC_PROJECT(vp,v,n) \ 385 { \ 386 GREAL scalar = VEC_DOT(v, n); \ 387 scalar/= VEC_DOT(n, n); \ 388 vp[0] = (scalar) * (n)[0]; \ 389 vp[1] = (scalar) * (n)[1]; \ 390 vp[2] = (scalar) * (n)[2]; \ 391 }\ 392 393 394 /*! accepts vector v*/ 395 #define VEC_UNPROJECT(vp,v,n) \ 396 { \ 397 GREAL scalar = VEC_DOT(v, n); \ 398 scalar = VEC_DOT(n, n)/scalar; \ 399 vp[0] = (scalar) * (n)[0]; \ 400 vp[1] = (scalar) * (n)[1]; \ 401 vp[2] = (scalar) * (n)[2]; \ 402 }\ 403 404 405 /*! Vector reflection -- assumes n is of unit length 406 Takes vector v, reflects it against reflector n, and returns vr */ 407 #define VEC_REFLECT(vr,v,n) \ 408 { \ 409 GREAL dot = VEC_DOT(v, n); \ 410 vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ 411 vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ 412 vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ 413 }\ 414 415 416 /*! Vector blending 417 Takes two vectors a, b, blends them together with two scalars */ 418 #define VEC_BLEND_AB(vr,sa,a,sb,b) \ 419 { \ 420 vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ 421 vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ 422 vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ 423 }\ 424 425 /*! Vector blending 426 Takes two vectors a, b, blends them together with s <=1 */ 427 #define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,1-s,a,sb,s) 428 429 #define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; 430 //! @} 431 432 433 /*! \defgroup MATRIX_OPERATIONS 434 Operations for matrices : mat2f, mat3f and mat4f 435 */ 436 //! @{ 437 438 /// initialize matrix 439 #define IDENTIFY_MATRIX_3X3(m) \ 440 { \ 441 m[0][0] = 1.0; \ 442 m[0][1] = 0.0; \ 443 m[0][2] = 0.0; \ 444 \ 445 m[1][0] = 0.0; \ 446 m[1][1] = 1.0; \ 447 m[1][2] = 0.0; \ 448 \ 449 m[2][0] = 0.0; \ 450 m[2][1] = 0.0; \ 451 m[2][2] = 1.0; \ 452 }\ 453 454 /*! initialize matrix */ 455 #define IDENTIFY_MATRIX_4X4(m) \ 456 { \ 457 m[0][0] = 1.0; \ 458 m[0][1] = 0.0; \ 459 m[0][2] = 0.0; \ 460 m[0][3] = 0.0; \ 461 \ 462 m[1][0] = 0.0; \ 463 m[1][1] = 1.0; \ 464 m[1][2] = 0.0; \ 465 m[1][3] = 0.0; \ 466 \ 467 m[2][0] = 0.0; \ 468 m[2][1] = 0.0; \ 469 m[2][2] = 1.0; \ 470 m[2][3] = 0.0; \ 471 \ 472 m[3][0] = 0.0; \ 473 m[3][1] = 0.0; \ 474 m[3][2] = 0.0; \ 475 m[3][3] = 1.0; \ 476 }\ 477 478 /*! initialize matrix */ 479 #define ZERO_MATRIX_4X4(m) \ 480 { \ 481 m[0][0] = 0.0; \ 482 m[0][1] = 0.0; \ 483 m[0][2] = 0.0; \ 484 m[0][3] = 0.0; \ 485 \ 486 m[1][0] = 0.0; \ 487 m[1][1] = 0.0; \ 488 m[1][2] = 0.0; \ 489 m[1][3] = 0.0; \ 490 \ 491 m[2][0] = 0.0; \ 492 m[2][1] = 0.0; \ 493 m[2][2] = 0.0; \ 494 m[2][3] = 0.0; \ 495 \ 496 m[3][0] = 0.0; \ 497 m[3][1] = 0.0; \ 498 m[3][2] = 0.0; \ 499 m[3][3] = 0.0; \ 500 }\ 501 502 /*! matrix rotation X */ 503 #define ROTX_CS(m,cosine,sine) \ 504 { \ 505 /* rotation about the x-axis */ \ 506 \ 507 m[0][0] = 1.0; \ 508 m[0][1] = 0.0; \ 509 m[0][2] = 0.0; \ 510 m[0][3] = 0.0; \ 511 \ 512 m[1][0] = 0.0; \ 513 m[1][1] = (cosine); \ 514 m[1][2] = (sine); \ 515 m[1][3] = 0.0; \ 516 \ 517 m[2][0] = 0.0; \ 518 m[2][1] = -(sine); \ 519 m[2][2] = (cosine); \ 520 m[2][3] = 0.0; \ 521 \ 522 m[3][0] = 0.0; \ 523 m[3][1] = 0.0; \ 524 m[3][2] = 0.0; \ 525 m[3][3] = 1.0; \ 526 }\ 527 528 /*! matrix rotation Y */ 529 #define ROTY_CS(m,cosine,sine) \ 530 { \ 531 /* rotation about the y-axis */ \ 532 \ 533 m[0][0] = (cosine); \ 534 m[0][1] = 0.0; \ 535 m[0][2] = -(sine); \ 536 m[0][3] = 0.0; \ 537 \ 538 m[1][0] = 0.0; \ 539 m[1][1] = 1.0; \ 540 m[1][2] = 0.0; \ 541 m[1][3] = 0.0; \ 542 \ 543 m[2][0] = (sine); \ 544 m[2][1] = 0.0; \ 545 m[2][2] = (cosine); \ 546 m[2][3] = 0.0; \ 547 \ 548 m[3][0] = 0.0; \ 549 m[3][1] = 0.0; \ 550 m[3][2] = 0.0; \ 551 m[3][3] = 1.0; \ 552 }\ 553 554 /*! matrix rotation Z */ 555 #define ROTZ_CS(m,cosine,sine) \ 556 { \ 557 /* rotation about the z-axis */ \ 558 \ 559 m[0][0] = (cosine); \ 560 m[0][1] = (sine); \ 561 m[0][2] = 0.0; \ 562 m[0][3] = 0.0; \ 563 \ 564 m[1][0] = -(sine); \ 565 m[1][1] = (cosine); \ 566 m[1][2] = 0.0; \ 567 m[1][3] = 0.0; \ 568 \ 569 m[2][0] = 0.0; \ 570 m[2][1] = 0.0; \ 571 m[2][2] = 1.0; \ 572 m[2][3] = 0.0; \ 573 \ 574 m[3][0] = 0.0; \ 575 m[3][1] = 0.0; \ 576 m[3][2] = 0.0; \ 577 m[3][3] = 1.0; \ 578 }\ 579 580 /*! matrix copy */ 581 #define COPY_MATRIX_2X2(b,a) \ 582 { \ 583 b[0][0] = a[0][0]; \ 584 b[0][1] = a[0][1]; \ 585 \ 586 b[1][0] = a[1][0]; \ 587 b[1][1] = a[1][1]; \ 588 \ 589 }\ 590 591 592 /*! matrix copy */ 593 #define COPY_MATRIX_2X3(b,a) \ 594 { \ 595 b[0][0] = a[0][0]; \ 596 b[0][1] = a[0][1]; \ 597 b[0][2] = a[0][2]; \ 598 \ 599 b[1][0] = a[1][0]; \ 600 b[1][1] = a[1][1]; \ 601 b[1][2] = a[1][2]; \ 602 }\ 603 604 605 /*! matrix copy */ 606 #define COPY_MATRIX_3X3(b,a) \ 607 { \ 608 b[0][0] = a[0][0]; \ 609 b[0][1] = a[0][1]; \ 610 b[0][2] = a[0][2]; \ 611 \ 612 b[1][0] = a[1][0]; \ 613 b[1][1] = a[1][1]; \ 614 b[1][2] = a[1][2]; \ 615 \ 616 b[2][0] = a[2][0]; \ 617 b[2][1] = a[2][1]; \ 618 b[2][2] = a[2][2]; \ 619 }\ 620 621 622 /*! matrix copy */ 623 #define COPY_MATRIX_4X4(b,a) \ 624 { \ 625 b[0][0] = a[0][0]; \ 626 b[0][1] = a[0][1]; \ 627 b[0][2] = a[0][2]; \ 628 b[0][3] = a[0][3]; \ 629 \ 630 b[1][0] = a[1][0]; \ 631 b[1][1] = a[1][1]; \ 632 b[1][2] = a[1][2]; \ 633 b[1][3] = a[1][3]; \ 634 \ 635 b[2][0] = a[2][0]; \ 636 b[2][1] = a[2][1]; \ 637 b[2][2] = a[2][2]; \ 638 b[2][3] = a[2][3]; \ 639 \ 640 b[3][0] = a[3][0]; \ 641 b[3][1] = a[3][1]; \ 642 b[3][2] = a[3][2]; \ 643 b[3][3] = a[3][3]; \ 644 }\ 645 646 647 /*! matrix transpose */ 648 #define TRANSPOSE_MATRIX_2X2(b,a) \ 649 { \ 650 b[0][0] = a[0][0]; \ 651 b[0][1] = a[1][0]; \ 652 \ 653 b[1][0] = a[0][1]; \ 654 b[1][1] = a[1][1]; \ 655 }\ 656 657 658 /*! matrix transpose */ 659 #define TRANSPOSE_MATRIX_3X3(b,a) \ 660 { \ 661 b[0][0] = a[0][0]; \ 662 b[0][1] = a[1][0]; \ 663 b[0][2] = a[2][0]; \ 664 \ 665 b[1][0] = a[0][1]; \ 666 b[1][1] = a[1][1]; \ 667 b[1][2] = a[2][1]; \ 668 \ 669 b[2][0] = a[0][2]; \ 670 b[2][1] = a[1][2]; \ 671 b[2][2] = a[2][2]; \ 672 }\ 673 674 675 /*! matrix transpose */ 676 #define TRANSPOSE_MATRIX_4X4(b,a) \ 677 { \ 678 b[0][0] = a[0][0]; \ 679 b[0][1] = a[1][0]; \ 680 b[0][2] = a[2][0]; \ 681 b[0][3] = a[3][0]; \ 682 \ 683 b[1][0] = a[0][1]; \ 684 b[1][1] = a[1][1]; \ 685 b[1][2] = a[2][1]; \ 686 b[1][3] = a[3][1]; \ 687 \ 688 b[2][0] = a[0][2]; \ 689 b[2][1] = a[1][2]; \ 690 b[2][2] = a[2][2]; \ 691 b[2][3] = a[3][2]; \ 692 \ 693 b[3][0] = a[0][3]; \ 694 b[3][1] = a[1][3]; \ 695 b[3][2] = a[2][3]; \ 696 b[3][3] = a[3][3]; \ 697 }\ 698 699 700 /*! multiply matrix by scalar */ 701 #define SCALE_MATRIX_2X2(b,s,a) \ 702 { \ 703 b[0][0] = (s) * a[0][0]; \ 704 b[0][1] = (s) * a[0][1]; \ 705 \ 706 b[1][0] = (s) * a[1][0]; \ 707 b[1][1] = (s) * a[1][1]; \ 708 }\ 709 710 711 /*! multiply matrix by scalar */ 712 #define SCALE_MATRIX_3X3(b,s,a) \ 713 { \ 714 b[0][0] = (s) * a[0][0]; \ 715 b[0][1] = (s) * a[0][1]; \ 716 b[0][2] = (s) * a[0][2]; \ 717 \ 718 b[1][0] = (s) * a[1][0]; \ 719 b[1][1] = (s) * a[1][1]; \ 720 b[1][2] = (s) * a[1][2]; \ 721 \ 722 b[2][0] = (s) * a[2][0]; \ 723 b[2][1] = (s) * a[2][1]; \ 724 b[2][2] = (s) * a[2][2]; \ 725 }\ 726 727 728 /*! multiply matrix by scalar */ 729 #define SCALE_MATRIX_4X4(b,s,a) \ 730 { \ 731 b[0][0] = (s) * a[0][0]; \ 732 b[0][1] = (s) * a[0][1]; \ 733 b[0][2] = (s) * a[0][2]; \ 734 b[0][3] = (s) * a[0][3]; \ 735 \ 736 b[1][0] = (s) * a[1][0]; \ 737 b[1][1] = (s) * a[1][1]; \ 738 b[1][2] = (s) * a[1][2]; \ 739 b[1][3] = (s) * a[1][3]; \ 740 \ 741 b[2][0] = (s) * a[2][0]; \ 742 b[2][1] = (s) * a[2][1]; \ 743 b[2][2] = (s) * a[2][2]; \ 744 b[2][3] = (s) * a[2][3]; \ 745 \ 746 b[3][0] = s * a[3][0]; \ 747 b[3][1] = s * a[3][1]; \ 748 b[3][2] = s * a[3][2]; \ 749 b[3][3] = s * a[3][3]; \ 750 }\ 751 752 753 /*! multiply matrix by scalar */ 754 #define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ 755 { \ 756 b[0][0] += (s) * a[0][0]; \ 757 b[0][1] += (s) * a[0][1]; \ 758 \ 759 b[1][0] += (s) * a[1][0]; \ 760 b[1][1] += (s) * a[1][1]; \ 761 }\ 762 763 764 /*! multiply matrix by scalar */ 765 #define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ 766 { \ 767 b[0][0] += (s) * a[0][0]; \ 768 b[0][1] += (s) * a[0][1]; \ 769 b[0][2] += (s) * a[0][2]; \ 770 \ 771 b[1][0] += (s) * a[1][0]; \ 772 b[1][1] += (s) * a[1][1]; \ 773 b[1][2] += (s) * a[1][2]; \ 774 \ 775 b[2][0] += (s) * a[2][0]; \ 776 b[2][1] += (s) * a[2][1]; \ 777 b[2][2] += (s) * a[2][2]; \ 778 }\ 779 780 781 /*! multiply matrix by scalar */ 782 #define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ 783 { \ 784 b[0][0] += (s) * a[0][0]; \ 785 b[0][1] += (s) * a[0][1]; \ 786 b[0][2] += (s) * a[0][2]; \ 787 b[0][3] += (s) * a[0][3]; \ 788 \ 789 b[1][0] += (s) * a[1][0]; \ 790 b[1][1] += (s) * a[1][1]; \ 791 b[1][2] += (s) * a[1][2]; \ 792 b[1][3] += (s) * a[1][3]; \ 793 \ 794 b[2][0] += (s) * a[2][0]; \ 795 b[2][1] += (s) * a[2][1]; \ 796 b[2][2] += (s) * a[2][2]; \ 797 b[2][3] += (s) * a[2][3]; \ 798 \ 799 b[3][0] += (s) * a[3][0]; \ 800 b[3][1] += (s) * a[3][1]; \ 801 b[3][2] += (s) * a[3][2]; \ 802 b[3][3] += (s) * a[3][3]; \ 803 }\ 804 805 /*! matrix product */ 806 /*! 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];*/ 807 #define MATRIX_PRODUCT_2X2(c,a,b) \ 808 { \ 809 c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ 810 c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ 811 \ 812 c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ 813 c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ 814 \ 815 }\ 816 817 /*! matrix product */ 818 /*! 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];*/ 819 #define MATRIX_PRODUCT_3X3(c,a,b) \ 820 { \ 821 c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ 822 c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ 823 c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ 824 \ 825 c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ 826 c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ 827 c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ 828 \ 829 c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ 830 c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ 831 c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ 832 }\ 833 834 835 /*! matrix product */ 836 /*! 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];*/ 837 #define MATRIX_PRODUCT_4X4(c,a,b) \ 838 { \ 839 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];\ 840 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];\ 841 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];\ 842 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];\ 843 \ 844 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];\ 845 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];\ 846 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];\ 847 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];\ 848 \ 849 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];\ 850 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];\ 851 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];\ 852 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];\ 853 \ 854 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];\ 855 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];\ 856 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];\ 857 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];\ 858 }\ 859 860 861 /*! matrix times vector */ 862 #define MAT_DOT_VEC_2X2(p,m,v) \ 863 { \ 864 p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ 865 p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ 866 }\ 867 868 869 /*! matrix times vector */ 870 #define MAT_DOT_VEC_3X3(p,m,v) \ 871 { \ 872 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ 873 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ 874 p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ 875 }\ 876 877 878 /*! matrix times vector 879 v is a vec4f 880 */ 881 #define MAT_DOT_VEC_4X4(p,m,v) \ 882 { \ 883 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ 884 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ 885 p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ 886 p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ 887 }\ 888 889 /*! matrix times vector 890 v is a vec3f 891 and m is a mat4f<br> 892 Last column is added as the position 893 */ 894 #define MAT_DOT_VEC_3X4(p,m,v) \ 895 { \ 896 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \ 897 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \ 898 p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \ 899 }\ 900 901 /*! vector transpose times matrix */ 902 /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ 903 #define VEC_DOT_MAT_3X3(p,v,m) \ 904 { \ 905 p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ 906 p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ 907 p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ 908 }\ 909 910 911 /*! affine matrix times vector */ 912 /** The matrix is assumed to be an affine matrix, with last two 913 * entries representing a translation */ 914 #define MAT_DOT_VEC_2X3(p,m,v) \ 915 { \ 916 p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ 917 p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ 918 }\ 919 920 921 /** inverse transpose of matrix times vector 922 * 923 * This macro computes inverse transpose of matrix m, 924 * and multiplies vector v into it, to yeild vector p 925 * 926 * DANGER !!! Do Not use this on normal vectors!!! 927 * It will leave normals the wrong length !!! 928 * See macro below for use on normals. 929 */ 930 #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ 931 { \ 932 GREAL det; \ 933 \ 934 det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ 935 p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ 936 p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ 937 \ 938 /* if matrix not singular, and not orthonormal, then renormalize */ \ 939 if ((det!=1.0f) && (det != 0.0f)) { \ 940 det = 1.0f / det; \ 941 p[0] *= det; \ 942 p[1] *= det; \ 943 } \ 944 }\ 945 946 947 /** transform normal vector by inverse transpose of matrix 948 * and then renormalize the vector 949 * 950 * This macro computes inverse transpose of matrix m, 951 * and multiplies vector v into it, to yeild vector p 952 * Vector p is then normalized. 953 */ 954 #define NORM_XFORM_2X2(p,m,v) \ 955 { \ 956 double len; \ 957 \ 958 /* do nothing if off-diagonals are zero and diagonals are \ 959 * equal */ \ 960 if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ 961 p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ 962 p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ 963 \ 964 len = p[0]*p[0] + p[1]*p[1]; \ 965 GIM_INV_SQRT(len,len); \ 966 p[0] *= len; \ 967 p[1] *= len; \ 968 } else { \ 969 VEC_COPY_2 (p, v); \ 970 } \ 971 }\ 972 973 974 /** outer product of vector times vector transpose 975 * 976 * The outer product of vector v and vector transpose t yeilds 977 * dyadic matrix m. 978 */ 979 #define OUTER_PRODUCT_2X2(m,v,t) \ 980 { \ 981 m[0][0] = v[0] * t[0]; \ 982 m[0][1] = v[0] * t[1]; \ 983 \ 984 m[1][0] = v[1] * t[0]; \ 985 m[1][1] = v[1] * t[1]; \ 986 }\ 987 988 989 /** outer product of vector times vector transpose 990 * 991 * The outer product of vector v and vector transpose t yeilds 992 * dyadic matrix m. 993 */ 994 #define OUTER_PRODUCT_3X3(m,v,t) \ 995 { \ 996 m[0][0] = v[0] * t[0]; \ 997 m[0][1] = v[0] * t[1]; \ 998 m[0][2] = v[0] * t[2]; \ 999 \ 1000 m[1][0] = v[1] * t[0]; \ 1001 m[1][1] = v[1] * t[1]; \ 1002 m[1][2] = v[1] * t[2]; \ 1003 \ 1004 m[2][0] = v[2] * t[0]; \ 1005 m[2][1] = v[2] * t[1]; \ 1006 m[2][2] = v[2] * t[2]; \ 1007 }\ 1008 1009 1010 /** outer product of vector times vector transpose 1011 * 1012 * The outer product of vector v and vector transpose t yeilds 1013 * dyadic matrix m. 1014 */ 1015 #define OUTER_PRODUCT_4X4(m,v,t) \ 1016 { \ 1017 m[0][0] = v[0] * t[0]; \ 1018 m[0][1] = v[0] * t[1]; \ 1019 m[0][2] = v[0] * t[2]; \ 1020 m[0][3] = v[0] * t[3]; \ 1021 \ 1022 m[1][0] = v[1] * t[0]; \ 1023 m[1][1] = v[1] * t[1]; \ 1024 m[1][2] = v[1] * t[2]; \ 1025 m[1][3] = v[1] * t[3]; \ 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 m[2][3] = v[2] * t[3]; \ 1031 \ 1032 m[3][0] = v[3] * t[0]; \ 1033 m[3][1] = v[3] * t[1]; \ 1034 m[3][2] = v[3] * t[2]; \ 1035 m[3][3] = v[3] * t[3]; \ 1036 }\ 1037 1038 1039 /** outer product of vector times vector transpose 1040 * 1041 * The outer product of vector v and vector transpose t yeilds 1042 * dyadic matrix m. 1043 */ 1044 #define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ 1045 { \ 1046 m[0][0] += v[0] * t[0]; \ 1047 m[0][1] += v[0] * t[1]; \ 1048 \ 1049 m[1][0] += v[1] * t[0]; \ 1050 m[1][1] += v[1] * t[1]; \ 1051 }\ 1052 1053 1054 /** outer product of vector times vector transpose 1055 * 1056 * The outer product of vector v and vector transpose t yeilds 1057 * dyadic matrix m. 1058 */ 1059 #define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ 1060 { \ 1061 m[0][0] += v[0] * t[0]; \ 1062 m[0][1] += v[0] * t[1]; \ 1063 m[0][2] += v[0] * t[2]; \ 1064 \ 1065 m[1][0] += v[1] * t[0]; \ 1066 m[1][1] += v[1] * t[1]; \ 1067 m[1][2] += v[1] * t[2]; \ 1068 \ 1069 m[2][0] += v[2] * t[0]; \ 1070 m[2][1] += v[2] * t[1]; \ 1071 m[2][2] += v[2] * t[2]; \ 1072 }\ 1073 1074 1075 /** outer product of vector times vector transpose 1076 * 1077 * The outer product of vector v and vector transpose t yeilds 1078 * dyadic matrix m. 1079 */ 1080 #define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ 1081 { \ 1082 m[0][0] += v[0] * t[0]; \ 1083 m[0][1] += v[0] * t[1]; \ 1084 m[0][2] += v[0] * t[2]; \ 1085 m[0][3] += v[0] * t[3]; \ 1086 \ 1087 m[1][0] += v[1] * t[0]; \ 1088 m[1][1] += v[1] * t[1]; \ 1089 m[1][2] += v[1] * t[2]; \ 1090 m[1][3] += v[1] * t[3]; \ 1091 \ 1092 m[2][0] += v[2] * t[0]; \ 1093 m[2][1] += v[2] * t[1]; \ 1094 m[2][2] += v[2] * t[2]; \ 1095 m[2][3] += v[2] * t[3]; \ 1096 \ 1097 m[3][0] += v[3] * t[0]; \ 1098 m[3][1] += v[3] * t[1]; \ 1099 m[3][2] += v[3] * t[2]; \ 1100 m[3][3] += v[3] * t[3]; \ 1101 }\ 1102 1103 1104 /** determinant of matrix 1105 * 1106 * Computes determinant of matrix m, returning d 1107 */ 1108 #define DETERMINANT_2X2(d,m) \ 1109 { \ 1110 d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ 1111 }\ 1112 1113 1114 /** determinant of matrix 1115 * 1116 * Computes determinant of matrix m, returning d 1117 */ 1118 #define DETERMINANT_3X3(d,m) \ 1119 { \ 1120 d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ 1121 d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ 1122 d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ 1123 }\ 1124 1125 1126 /** i,j,th cofactor of a 4x4 matrix 1127 * 1128 */ 1129 #define COFACTOR_4X4_IJ(fac,m,i,j) \ 1130 { \ 1131 int __ii[4], __jj[4], __k; \ 1132 \ 1133 for (__k=0; __k<i; __k++) __ii[__k] = __k; \ 1134 for (__k=i; __k<3; __k++) __ii[__k] = __k+1; \ 1135 for (__k=0; __k<j; __k++) __jj[__k] = __k; \ 1136 for (__k=j; __k<3; __k++) __jj[__k] = __k+1; \ 1137 \ 1138 (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] \ 1139 - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \ 1140 (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]] \ 1141 - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\ 1142 (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]] \ 1143 - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\ 1144 \ 1145 __k = i+j; \ 1146 if ( __k != (__k/2)*2) { \ 1147 (fac) = -(fac); \ 1148 } \ 1149 }\ 1150 1151 1152 /** determinant of matrix 1153 * 1154 * Computes determinant of matrix m, returning d 1155 */ 1156 #define DETERMINANT_4X4(d,m) \ 1157 { \ 1158 double cofac; \ 1159 COFACTOR_4X4_IJ (cofac, m, 0, 0); \ 1160 d = m[0][0] * cofac; \ 1161 COFACTOR_4X4_IJ (cofac, m, 0, 1); \ 1162 d += m[0][1] * cofac; \ 1163 COFACTOR_4X4_IJ (cofac, m, 0, 2); \ 1164 d += m[0][2] * cofac; \ 1165 COFACTOR_4X4_IJ (cofac, m, 0, 3); \ 1166 d += m[0][3] * cofac; \ 1167 }\ 1168 1169 1170 /** cofactor of matrix 1171 * 1172 * Computes cofactor of matrix m, returning a 1173 */ 1174 #define COFACTOR_2X2(a,m) \ 1175 { \ 1176 a[0][0] = (m)[1][1]; \ 1177 a[0][1] = - (m)[1][0]; \ 1178 a[1][0] = - (m)[0][1]; \ 1179 a[1][1] = (m)[0][0]; \ 1180 }\ 1181 1182 1183 /** cofactor of matrix 1184 * 1185 * Computes cofactor of matrix m, returning a 1186 */ 1187 #define COFACTOR_3X3(a,m) \ 1188 { \ 1189 a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ 1190 a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ 1191 a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ 1192 a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ 1193 a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ 1194 a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ 1195 a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ 1196 a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ 1197 a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ 1198 }\ 1199 1200 1201 /** cofactor of matrix 1202 * 1203 * Computes cofactor of matrix m, returning a 1204 */ 1205 #define COFACTOR_4X4(a,m) \ 1206 { \ 1207 int i,j; \ 1208 \ 1209 for (i=0; i<4; i++) { \ 1210 for (j=0; j<4; j++) { \ 1211 COFACTOR_4X4_IJ (a[i][j], m, i, j); \ 1212 } \ 1213 } \ 1214 }\ 1215 1216 1217 /** adjoint of matrix 1218 * 1219 * Computes adjoint of matrix m, returning a 1220 * (Note that adjoint is just the transpose of the cofactor matrix) 1221 */ 1222 #define ADJOINT_2X2(a,m) \ 1223 { \ 1224 a[0][0] = (m)[1][1]; \ 1225 a[1][0] = - (m)[1][0]; \ 1226 a[0][1] = - (m)[0][1]; \ 1227 a[1][1] = (m)[0][0]; \ 1228 }\ 1229 1230 1231 /** adjoint of matrix 1232 * 1233 * Computes adjoint of matrix m, returning a 1234 * (Note that adjoint is just the transpose of the cofactor matrix) 1235 */ 1236 #define ADJOINT_3X3(a,m) \ 1237 { \ 1238 a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ 1239 a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ 1240 a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ 1241 a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ 1242 a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ 1243 a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ 1244 a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ 1245 a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ 1246 a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ 1247 }\ 1248 1249 1250 /** adjoint of matrix 1251 * 1252 * Computes adjoint of matrix m, returning a 1253 * (Note that adjoint is just the transpose of the cofactor matrix) 1254 */ 1255 #define ADJOINT_4X4(a,m) \ 1256 { \ 1257 char _i_,_j_; \ 1258 \ 1259 for (_i_=0; _i_<4; _i_++) { \ 1260 for (_j_=0; _j_<4; _j_++) { \ 1261 COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ 1262 } \ 1263 } \ 1264 }\ 1265 1266 1267 /** compute adjoint of matrix and scale 1268 * 1269 * Computes adjoint of matrix m, scales it by s, returning a 1270 */ 1271 #define SCALE_ADJOINT_2X2(a,s,m) \ 1272 { \ 1273 a[0][0] = (s) * m[1][1]; \ 1274 a[1][0] = - (s) * m[1][0]; \ 1275 a[0][1] = - (s) * m[0][1]; \ 1276 a[1][1] = (s) * m[0][0]; \ 1277 }\ 1278 1279 1280 /** compute adjoint of matrix and scale 1281 * 1282 * Computes adjoint of matrix m, scales it by s, returning a 1283 */ 1284 #define SCALE_ADJOINT_3X3(a,s,m) \ 1285 { \ 1286 a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ 1287 a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ 1288 a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ 1289 \ 1290 a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ 1291 a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ 1292 a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ 1293 \ 1294 a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ 1295 a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ 1296 a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ 1297 }\ 1298 1299 1300 /** compute adjoint of matrix and scale 1301 * 1302 * Computes adjoint of matrix m, scales it by s, returning a 1303 */ 1304 #define SCALE_ADJOINT_4X4(a,s,m) \ 1305 { \ 1306 char _i_,_j_; \ 1307 for (_i_=0; _i_<4; _i_++) { \ 1308 for (_j_=0; _j_<4; _j_++) { \ 1309 COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ 1310 a[_j_][_i_] *= s; \ 1311 } \ 1312 } \ 1313 }\ 1314 1315 /** inverse of matrix 1316 * 1317 * Compute inverse of matrix a, returning determinant m and 1318 * inverse b 1319 */ 1320 #define INVERT_2X2(b,det,a) \ 1321 { \ 1322 GREAL _tmp_; \ 1323 DETERMINANT_2X2 (det, a); \ 1324 _tmp_ = 1.0 / (det); \ 1325 SCALE_ADJOINT_2X2 (b, _tmp_, a); \ 1326 }\ 1327 1328 1329 /** inverse of matrix 1330 * 1331 * Compute inverse of matrix a, returning determinant m and 1332 * inverse b 1333 */ 1334 #define INVERT_3X3(b,det,a) \ 1335 { \ 1336 GREAL _tmp_; \ 1337 DETERMINANT_3X3 (det, a); \ 1338 _tmp_ = 1.0 / (det); \ 1339 SCALE_ADJOINT_3X3 (b, _tmp_, a); \ 1340 }\ 1341 1342 1343 /** inverse of matrix 1344 * 1345 * Compute inverse of matrix a, returning determinant m and 1346 * inverse b 1347 */ 1348 #define INVERT_4X4(b,det,a) \ 1349 { \ 1350 GREAL _tmp_; \ 1351 DETERMINANT_4X4 (det, a); \ 1352 _tmp_ = 1.0 / (det); \ 1353 SCALE_ADJOINT_4X4 (b, _tmp_, a); \ 1354 }\ 1355 1356 //! @} 1357 1358 /*! \defgroup BOUND_AABB_OPERATIONS 1359 */ 1360 //! @{ 1361 1362 //!Initializes an AABB 1363 #define INVALIDATE_AABB(aabb) {\ 1364 (aabb).minX = G_REAL_INFINITY;\ 1365 (aabb).maxX = -G_REAL_INFINITY;\ 1366 (aabb).minY = G_REAL_INFINITY;\ 1367 (aabb).maxY = -G_REAL_INFINITY;\ 1368 (aabb).minZ = G_REAL_INFINITY;\ 1369 (aabb).maxZ = -G_REAL_INFINITY;\ 1370 }\ 1371 1372 #define AABB_GET_MIN(aabb,vmin) {\ 1373 vmin[0] = (aabb).minX;\ 1374 vmin[1] = (aabb).minY;\ 1375 vmin[2] = (aabb).minZ;\ 1376 }\ 1377 1378 #define AABB_GET_MAX(aabb,vmax) {\ 1379 vmax[0] = (aabb).maxX;\ 1380 vmax[1] = (aabb).maxY;\ 1381 vmax[2] = (aabb).maxZ;\ 1382 }\ 1383 1384 //!Copy boxes 1385 #define AABB_COPY(dest_aabb,src_aabb)\ 1386 {\ 1387 (dest_aabb).minX = (src_aabb).minX;\ 1388 (dest_aabb).maxX = (src_aabb).maxX;\ 1389 (dest_aabb).minY = (src_aabb).minY;\ 1390 (dest_aabb).maxY = (src_aabb).maxY;\ 1391 (dest_aabb).minZ = (src_aabb).minZ;\ 1392 (dest_aabb).maxZ = (src_aabb).maxZ;\ 1393 }\ 1394 1395 //! Computes an Axis aligned box from a triangle 1396 #define COMPUTEAABB_FOR_TRIANGLE(aabb,V1,V2,V3) {\ 1397 (aabb).minX = MIN3(V1[0],V2[0],V3[0]);\ 1398 (aabb).maxX = MAX3(V1[0],V2[0],V3[0]);\ 1399 (aabb).minY = MIN3(V1[1],V2[1],V3[1]);\ 1400 (aabb).maxY = MAX3(V1[1],V2[1],V3[1]);\ 1401 (aabb).minZ = MIN3(V1[2],V2[2],V3[2]);\ 1402 (aabb).maxZ = MAX3(V1[2],V2[2],V3[2]);\ 1403 }\ 1404 1405 //! Merge two boxes to destaabb 1406 #define MERGEBOXES(destaabb,aabb) {\ 1407 (destaabb).minX = MIN((aabb).minX,(destaabb).minX);\ 1408 (destaabb).minY = MIN((aabb).minY,(destaabb).minY);\ 1409 (destaabb).minZ = MIN((aabb).minZ,(destaabb).minZ);\ 1410 (destaabb).maxX = MAX((aabb).maxX,(destaabb).maxX);\ 1411 (destaabb).maxY = MAX((aabb).maxY,(destaabb).maxY);\ 1412 (destaabb).maxZ = MAX((aabb).maxZ,(destaabb).maxZ);\ 1413 }\ 1414 1415 //! Extends the box 1416 #define AABB_POINT_EXTEND(destaabb,p) {\ 1417 (destaabb).minX = MIN(p[0],(destaabb).minX);\ 1418 (destaabb).maxX = MAX(p[0],(destaabb).maxX);\ 1419 (destaabb).minY = MIN(p[1],(destaabb).minY);\ 1420 (destaabb).maxY = MAX(p[1],(destaabb).maxY);\ 1421 (destaabb).minZ = MIN(p[2],(destaabb).minZ);\ 1422 (destaabb).maxZ = MAX(p[2],(destaabb).maxZ);\ 1423 }\ 1424 1425 //! Finds the intersection box of two boxes 1426 #define BOXINTERSECTION(aabb1, aabb2, iaabb) {\ 1427 (iaabb).minX = MAX((aabb1).minX,(aabb2).minX);\ 1428 (iaabb).minY = MAX((aabb1).minY,(aabb2).minY);\ 1429 (iaabb).minZ = MAX((aabb1).minZ,(aabb2).minZ);\ 1430 (iaabb).maxX = MIN((aabb1).maxX,(aabb2).maxX);\ 1431 (iaabb).maxY = MIN((aabb1).maxY,(aabb2).maxY);\ 1432 (iaabb).maxZ = MIN((aabb1).maxZ,(aabb2).maxZ);\ 1433 }\ 1434 1435 //! Determines if two aligned boxes do intersect 1436 #define AABBCOLLISION(intersected,aabb1,aabb2) {\ 1437 intersected = 1;\ 1438 if ((aabb1).minX > (aabb2).maxX ||\ 1439 (aabb1).maxX < (aabb2).minX ||\ 1440 (aabb1).minY > (aabb2).maxY ||\ 1441 (aabb1).maxY < (aabb2).minY ||\ 1442 (aabb1).minZ > (aabb2).maxZ ||\ 1443 (aabb1).maxZ < (aabb2).minZ )\ 1444 {\ 1445 intersected = 0;\ 1446 }\ 1447 }\ 1448 1449 #define AXIS_INTERSECT(min,max, a, d,tfirst, tlast,is_intersected) {\ 1450 if(IS_ZERO(d))\ 1451 {\ 1452 is_intersected = !(a < min || a > max);\ 1453 }\ 1454 else\ 1455 {\ 1456 GREAL a0, a1;\ 1457 a0 = (min - a) / (d);\ 1458 a1 = (max - a) / (d);\ 1459 if(a0 > a1) SWAP_NUMBERS(a0, a1);\ 1460 tfirst = MAX(a0, tfirst);\ 1461 tlast = MIN(a1, tlast);\ 1462 if (tlast < tfirst)\ 1463 {\ 1464 is_intersected = 0;\ 1465 }\ 1466 else\ 1467 {\ 1468 is_intersected = 1;\ 1469 }\ 1470 }\ 1471 }\ 1472 1473 /*! \brief Finds the Ray intersection parameter. 1474 1475 \param aabb Aligned box 1476 \param vorigin A vec3f with the origin of the ray 1477 \param vdir A vec3f with the direction of the ray 1478 \param tparam Output parameter 1479 \param tmax Max lenght of the ray 1480 \param is_intersected 1 if the ray collides the box, else false 1481 1482 */ 1483 #define BOX_INTERSECTS_RAY(aabb, vorigin, vdir, tparam, tmax,is_intersected) { \ 1484 GREAL _tfirst = 0.0f, _tlast = tmax;\ 1485 AXIS_INTERSECT(aabb.minX,aabb.maxX,vorigin[0], vdir[0], _tfirst, _tlast,is_intersected);\ 1486 if(is_intersected)\ 1487 {\ 1488 AXIS_INTERSECT(aabb.minY,aabb.maxY,vorigin[1], vdir[1], _tfirst, _tlast,is_intersected);\ 1489 }\ 1490 if(is_intersected)\ 1491 {\ 1492 AXIS_INTERSECT(aabb.minZ,aabb.maxZ,vorigin[2], vdir[2], _tfirst, _tlast,is_intersected);\ 1493 }\ 1494 tparam = _tfirst;\ 1495 }\ 1496 1497 #define AABB_PROJECTION_INTERVAL(aabb,direction, vmin, vmax)\ 1498 {\ 1499 GREAL _center[] = {(aabb.minX + aabb.maxX)*0.5f, (aabb.minY + aabb.maxY)*0.5f, (aabb.minZ + aabb.maxZ)*0.5f};\ 1500 \ 1501 GREAL _extend[] = {aabb.maxX-_center[0],aabb.maxY-_center[1],aabb.maxZ-_center[2]};\ 1502 GREAL _fOrigin = VEC_DOT(direction,_center);\ 1503 GREAL _fMaximumExtent = _extend[0]*fabsf(direction[0]) + \ 1504 _extend[1]*fabsf(direction[1]) + \ 1505 _extend[2]*fabsf(direction[2]); \ 1506 \ 1507 vmin = _fOrigin - _fMaximumExtent; \ 1508 vmax = _fOrigin + _fMaximumExtent; \ 1509 }\ 1510 1511 /*! 1512 classify values: 1513 <ol> 1514 <li> 0 : In back of plane 1515 <li> 1 : Spanning 1516 <li> 2 : In front of 1517 </ol> 1518 */ 1519 #define PLANE_CLASSIFY_BOX(plane,aabb,classify)\ 1520 {\ 1521 GREAL _fmin,_fmax; \ 1522 AABB_PROJECTION_INTERVAL(aabb,plane, _fmin, _fmax); \ 1523 if(plane[3] >= _fmax) \ 1524 { \ 1525 classify = 0;/*In back of*/ \ 1526 } \ 1527 else \ 1528 { \ 1529 if(plane[3]+0.000001f>=_fmin) \ 1530 { \ 1531 classify = 1;/*Spanning*/ \ 1532 } \ 1533 else \ 1534 { \ 1535 classify = 2;/*In front of*/ \ 1536 } \ 1537 } \ 1538 }\ 1539 //! @} 1540 1541 /*! \defgroup GEOMETRIC_OPERATIONS 1542 */ 1543 //! @{ 1544 1545 1546 #define PLANEDIREPSILON 0.0000001f 1547 #define PARALELENORMALS 0.000001f 1548 1549 #define TRIANGLE_NORMAL(v1,v2,v3,n){\ 1550 vec3f _dif1,_dif2; \ 1551 VEC_DIFF(_dif1,v2,v1); \ 1552 VEC_DIFF(_dif2,v3,v1); \ 1553 VEC_CROSS(n,_dif1,_dif2); \ 1554 VEC_NORMALIZE(n); \ 1555 }\ 1556 1557 /// plane is a vec4f 1558 #define TRIANGLE_PLANE(v1,v2,v3,plane) {\ 1559 TRIANGLE_NORMAL(v1,v2,v3,plane);\ 1560 plane[3] = VEC_DOT(v1,plane);\ 1561 }\ 1562 1563 /// Calc a plane from an edge an a normal. plane is a vec4f 1564 #define EDGE_PLANE(e1,e2,n,plane) {\ 1565 vec3f _dif; \ 1566 VEC_DIFF(_dif,e2,e1); \ 1567 VEC_CROSS(plane,_dif,n); \ 1568 VEC_NORMALIZE(plane); \ 1569 plane[3] = VEC_DOT(e1,plane);\ 1570 }\ 1571 1572 #define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3]) 1573 1574 #define PROJECT_POINT_PLANE(point,plane,projected) {\ 1575 GREAL _dis;\ 1576 _dis = DISTANCE_PLANE_POINT(plane,point);\ 1577 VEC_SCALE(projected,-_dis,plane);\ 1578 VEC_SUM(projected,projected,point); \ 1579 }\ 1580 1581 #define POINT_IN_HULL(point,planes,plane_count,outside)\ 1582 {\ 1583 GREAL _dis;\ 1584 outside = 0;\ 1585 GUINT32 _i = 0;\ 1586 do\ 1587 {\ 1588 _dis = DISTANCE_PLANE_POINT(planes[_i],point);\ 1589 if(_dis>0.0f) outside = 1;\ 1590 _i++;\ 1591 }while(_i<plane_count&&outside==0);\ 1592 }\ 1593 1594 1595 #define PLANE_CLIP_SEGMENT(s1,s2,plane,clipped) {\ 1596 GREAL _dis1,_dis2;\ 1597 _dis1 = DISTANCE_PLANE_POINT(plane,s1);\ 1598 VEC_DIFF(clipped,s2,s1);\ 1599 _dis2 = VEC_DOT(clipped,plane);\ 1600 VEC_SCALE(clipped,-_dis1/_dis2,clipped);\ 1601 VEC_SUM(clipped,clipped,s1); \ 1602 }\ 1603 1604 //! Confirms if the plane intersect the edge or nor 1605 /*! 1606 intersection type must have the following values 1607 <ul> 1608 <li> 0 : Segment in front of plane, s1 closest 1609 <li> 1 : Segment in front of plane, s2 closest 1610 <li> 2 : Segment in back of plane, s1 closest 1611 <li> 3 : Segment in back of plane, s2 closest 1612 <li> 4 : Segment collides plane, s1 in back 1613 <li> 5 : Segment collides plane, s2 in back 1614 </ul> 1615 */ 1616 #define PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped,intersection_type) \ 1617 {\ 1618 GREAL _dis1,_dis2;\ 1619 _dis1 = DISTANCE_PLANE_POINT(plane,s1);\ 1620 _dis2 = DISTANCE_PLANE_POINT(plane,s2);\ 1621 if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)\ 1622 {\ 1623 if(_dis1<_dis2) intersection_type = 0;\ 1624 else intersection_type = 1;\ 1625 }\ 1626 else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)\ 1627 {\ 1628 if(_dis1>_dis2) intersection_type = 2;\ 1629 else intersection_type = 3; \ 1630 }\ 1631 else\ 1632 {\ 1633 if(_dis1<_dis2) intersection_type = 4;\ 1634 else intersection_type = 5;\ 1635 VEC_DIFF(clipped,s2,s1);\ 1636 _dis2 = VEC_DOT(clipped,plane);\ 1637 VEC_SCALE(clipped,-_dis1/_dis2,clipped);\ 1638 VEC_SUM(clipped,clipped,s1); \ 1639 }\ 1640 }\ 1641 1642 //! Confirms if the plane intersect the edge or not 1643 /*! 1644 clipped1 and clipped2 are the vertices behind the plane. 1645 clipped1 is the closest 1646 1647 intersection_type must have the following values 1648 <ul> 1649 <li> 0 : Segment in front of plane, s1 closest 1650 <li> 1 : Segment in front of plane, s2 closest 1651 <li> 2 : Segment in back of plane, s1 closest 1652 <li> 3 : Segment in back of plane, s2 closest 1653 <li> 4 : Segment collides plane, s1 in back 1654 <li> 5 : Segment collides plane, s2 in back 1655 </ul> 1656 */ 1657 #define PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,plane,clipped1,clipped2,intersection_type)\ 1658 {\ 1659 PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1,intersection_type);\ 1660 if(intersection_type == 0)\ 1661 {\ 1662 VEC_COPY(clipped1,s1);\ 1663 VEC_COPY(clipped2,s2);\ 1664 }\ 1665 else if(intersection_type == 1)\ 1666 {\ 1667 VEC_COPY(clipped1,s2);\ 1668 VEC_COPY(clipped2,s1);\ 1669 }\ 1670 else if(intersection_type == 2)\ 1671 {\ 1672 VEC_COPY(clipped1,s1);\ 1673 VEC_COPY(clipped2,s2);\ 1674 }\ 1675 else if(intersection_type == 3)\ 1676 {\ 1677 VEC_COPY(clipped1,s2);\ 1678 VEC_COPY(clipped2,s1);\ 1679 }\ 1680 else if(intersection_type == 4)\ 1681 { \ 1682 VEC_COPY(clipped2,s1);\ 1683 }\ 1684 else if(intersection_type == 5)\ 1685 { \ 1686 VEC_COPY(clipped2,s2);\ 1687 }\ 1688 }\ 1689 1690 1691 //! Finds the 2 smallest cartesian coordinates of a plane normal 1692 #define PLANE_MINOR_AXES(plane, i0, i1)\ 1693 {\ 1694 GREAL A[] = {fabs(plane[0]),fabs(plane[1]),fabs(plane[2])};\ 1695 if(A[0]>A[1])\ 1696 {\ 1697 if(A[0]>A[2])\ 1698 {\ 1699 i0=1; /* A[0] is greatest */ \ 1700 i1=2;\ 1701 }\ 1702 else \ 1703 {\ 1704 i0=0; /* A[2] is greatest */ \ 1705 i1=1; \ 1706 }\ 1707 }\ 1708 else /* A[0]<=A[1] */ \ 1709 {\ 1710 if(A[2]>A[1]) \ 1711 { \ 1712 i0=0; /* A[2] is greatest */ \ 1713 i1=1; \ 1714 }\ 1715 else \ 1716 {\ 1717 i0=0; /* A[1] is greatest */ \ 1718 i1=2; \ 1719 }\ 1720 } \ 1721 }\ 1722 1723 //! Ray plane collision 1724 #define RAY_PLANE_COLLISION(plane,vDir,vPoint,pout,tparam,does_intersect)\ 1725 {\ 1726 GREAL _dis,_dotdir; \ 1727 _dotdir = VEC_DOT(plane,vDir);\ 1728 if(_dotdir<PLANEDIREPSILON)\ 1729 {\ 1730 does_intersect = 0;\ 1731 }\ 1732 else\ 1733 {\ 1734 _dis = DISTANCE_PLANE_POINT(plane,vPoint); \ 1735 tparam = -_dis/_dotdir;\ 1736 VEC_SCALE(pout,tparam,vDir);\ 1737 VEC_SUM(pout,vPoint,pout); \ 1738 does_intersect = 1;\ 1739 }\ 1740 }\ 1741 1742 //! Bidireccional ray 1743 #define LINE_PLANE_COLLISION(plane,vDir,vPoint,pout,tparam, tmin, tmax)\ 1744 {\ 1745 tparam = -DISTANCE_PLANE_POINT(plane,vPoint);\ 1746 tparam /= VEC_DOT(plane,vDir);\ 1747 tparam = CLAMP(tparam,tmin,tmax);\ 1748 VEC_SCALE(pout,tparam,vDir);\ 1749 VEC_SUM(pout,vPoint,pout); \ 1750 }\ 1751 1752 /*! \brief Returns the Ray on which 2 planes intersect if they do. 1753 Written by Rodrigo Hernandez on ODE convex collision 1754 1755 \param p1 Plane 1 1756 \param p2 Plane 2 1757 \param p Contains the origin of the ray upon returning if planes intersect 1758 \param d Contains the direction of the ray upon returning if planes intersect 1759 \param dointersect 1 if the planes intersect, 0 if paralell. 1760 1761 */ 1762 #define INTERSECT_PLANES(p1,p2,p,d,dointersect) \ 1763 { \ 1764 VEC_CROSS(d,p1,p2); \ 1765 GREAL denom = VEC_DOT(d, d);\ 1766 if (IS_ZERO(denom)) \ 1767 { \ 1768 dointersect = 0; \ 1769 } \ 1770 else \ 1771 { \ 1772 vec3f _n;\ 1773 _n[0]=p1[3]*p2[0] - p2[3]*p1[0]; \ 1774 _n[1]=p1[3]*p2[1] - p2[3]*p1[1]; \ 1775 _n[2]=p1[3]*p2[2] - p2[3]*p1[2]; \ 1776 VEC_CROSS(p,_n,d); \ 1777 p[0]/=denom; \ 1778 p[1]/=denom; \ 1779 p[2]/=denom; \ 1780 dointersect = 1; \ 1781 }\ 1782 }\ 1783 1784 //***************** SEGMENT and LINE FUNCTIONS **********************************/// 1785 1786 /*! Finds the closest point(cp) to (v) on a segment (e1,e2) 1787 */ 1788 #define CLOSEST_POINT_ON_SEGMENT(cp,v,e1,e2) \ 1789 { \ 1790 vec3f _n;\ 1791 VEC_DIFF(_n,e2,e1);\ 1792 VEC_DIFF(cp,v,e1);\ 1793 GREAL _scalar = VEC_DOT(cp, _n); \ 1794 _scalar/= VEC_DOT(_n, _n); \ 1795 if(_scalar <0.0f)\ 1796 {\ 1797 VEC_COPY(cp,e1);\ 1798 }\ 1799 else if(_scalar >1.0f)\ 1800 {\ 1801 VEC_COPY(cp,e2);\ 1802 }\ 1803 else \ 1804 {\ 1805 VEC_SCALE(cp,_scalar,_n);\ 1806 VEC_SUM(cp,cp,e1);\ 1807 } \ 1808 }\ 1809 1810 1811 /*! \brief Finds the line params where these lines intersect. 1812 1813 \param dir1 Direction of line 1 1814 \param point1 Point of line 1 1815 \param dir2 Direction of line 2 1816 \param point2 Point of line 2 1817 \param t1 Result Parameter for line 1 1818 \param t2 Result Parameter for line 2 1819 \param dointersect 0 if the lines won't intersect, else 1 1820 1821 */ 1822 #define LINE_INTERSECTION_PARAMS(dir1,point1, dir2, point2,t1,t2,dointersect) {\ 1823 GREAL det;\ 1824 GREAL e1e1 = VEC_DOT(dir1,dir1);\ 1825 GREAL e1e2 = VEC_DOT(dir1,dir2);\ 1826 GREAL e2e2 = VEC_DOT(dir2,dir2);\ 1827 vec3f p1p2;\ 1828 VEC_DIFF(p1p2,point1,point2);\ 1829 GREAL p1p2e1 = VEC_DOT(p1p2,dir1);\ 1830 GREAL p1p2e2 = VEC_DOT(p1p2,dir2);\ 1831 det = e1e2*e1e2 - e1e1*e2e2;\ 1832 if(IS_ZERO(det))\ 1833 {\ 1834 dointersect = 0;\ 1835 }\ 1836 else\ 1837 {\ 1838 t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;\ 1839 t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;\ 1840 dointersect = 1;\ 1841 }\ 1842 }\ 1843 1844 //! Find closest points on segments 1845 #define SEGMENT_COLLISION(vA1,vA2,vB1,vB2,vPointA,vPointB)\ 1846 {\ 1847 vec3f _AD,_BD,_N;\ 1848 vec4f _M;\ 1849 VEC_DIFF(_AD,vA2,vA1);\ 1850 VEC_DIFF(_BD,vB2,vB1);\ 1851 VEC_CROSS(_N,_AD,_BD);\ 1852 VEC_CROSS(_M,_N,_BD);\ 1853 _M[3] = VEC_DOT(_M,vB1);\ 1854 float _tp; \ 1855 LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,0.0f, 1.0f);\ 1856 /*Closest point on segment*/ \ 1857 VEC_DIFF(vPointB,vPointA,vB1);\ 1858 _tp = VEC_DOT(vPointB, _BD); \ 1859 _tp/= VEC_DOT(_BD, _BD); \ 1860 _tp = CLAMP(_tp,0.0f,1.0f); \ 1861 VEC_SCALE(vPointB,_tp,_BD);\ 1862 VEC_SUM(vPointB,vPointB,vB1);\ 1863 }\ 1864 1865 //! @} 1866 1867 ///Additional Headers for Collision 1868 #include "GIMPACT/gim_tri_collision.h" 1869 #include "GIMPACT/gim_tri_sphere_collision.h" 1870 #include "GIMPACT/gim_tri_capsule_collision.h" 1871 1872 #endif // GIM_VECTOR_H_INCLUDED 1873