00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 #ifndef __VMATH_H__
00074 #define __VMATH_H__
00075
00076 #include "common.h"
00077
00078
00079 #include <math.h>
00080
00081
00082 #include <float.h>
00083
00084
00085 #include "bu.h"
00086
00087
00088 __BEGIN_DECLS
00089
00090 #ifndef M_
00091 # define M_ XXX
00092 #endif
00093
00094 #ifndef M_1_PI
00095 # define M_1_PI 0.31830988618379067153776752675
00096 #endif
00097 #ifndef M_2_PI
00098 # define M_2_PI 0.63661977236758134307553505349
00099 #endif
00100 #ifndef M_2_SQRTPI
00101 # define M_2_SQRTPI 1.12837916709551257389615890312
00102 #endif
00103 #ifndef M_E
00104 # define M_E 2.71828182845904523536028747135
00105 #endif
00106 #ifndef M_EULER
00107 # define M_EULER 0.57721566490153286060651209008
00108 #endif
00109 #ifndef M_LOG2E
00110 # define M_LOG2E 1.44269504088896340735992468100
00111 #endif
00112 #ifndef M_LOG10E
00113 # define M_LOG10E 0.43429448190325182765112891892
00114 #endif
00115 #ifndef M_LN10
00116 # define M_LN10 2.30258509299404568401799145468
00117 #endif
00118 #ifndef M_LN2
00119 # define M_LN2 0.69314718055994530941723212146
00120 #endif
00121 #ifndef M_LNPI
00122 # define M_LNPI 1.14472988584940017414342735135
00123 #endif
00124 #ifndef M_PI
00125 # define M_PI 3.14159265358979323846264338328
00126 #endif
00127 #ifndef M_PI_2
00128 # define M_PI_2 1.57079632679489661923132169164
00129 #endif
00130 #ifndef M_PI_4
00131 # define M_PI_4 0.78539816339744830966156608458
00132 #endif
00133 #ifndef M_SQRT1_2
00134 # define M_SQRT1_2 0.70710678118654752440084436210
00135 #endif
00136 #ifndef M_SQRT2
00137 # define M_SQRT2 1.41421356237309504880168872421
00138 #endif
00139 #ifndef M_SQRT3
00140 # define M_SQRT3 1.73205080756887729352744634151
00141 #endif
00142 #ifndef M_SQRTPI
00143 # define M_SQRTPI 1.77245385090551602729816748334
00144 #endif
00145
00146 #ifndef DEG2RAD
00147 # define DEG2RAD 0.017453292519943295769236907684
00148 #endif
00149 #ifndef RAD2DEG
00150 # define RAD2DEG 57.295779513082320876798154814105
00151 #endif
00152
00153
00154
00155 #ifdef vax
00156 # define VDIVIDE_TOL (1.0e-10)
00157 # define VUNITIZE_TOL (1.0e-7)
00158 #else
00159 # ifdef DBL_EPSILON
00160 # define VDIVIDE_TOL (DBL_EPSILON)
00161 # else
00162 # define VDIVIDE_TOL (1.0e-20)
00163 # endif
00164 # ifdef FLT_EPSILON
00165 # define VUNITIZE_TOL (FLT_EPSILON)
00166 # else
00167 # define VUNITIZE_TOL (1.0e-15)
00168 # endif
00169 #endif
00170
00171
00172 #define ELEMENTS_PER_VECT2D 2
00173
00174
00175 #define ELEMENTS_PER_POINT2D 2
00176
00177
00178 #define ELEMENTS_PER_VECT 3
00179
00180
00181 #define ELEMENTS_PER_POINT 3
00182
00183
00184 #define ELEMENTS_PER_HVECT 4
00185
00186
00187 #define ELEMENTS_PER_HPOINT 4
00188
00189
00190 #define ELEMENTS_PER_PLANE 4
00191
00192
00193 #define ELEMENTS_PER_MAT (ELEMENTS_PER_PLANE*ELEMENTS_PER_PLANE)
00194
00195
00196
00197
00198
00199
00200 typedef fastf_t vect2d_t[ELEMENTS_PER_VECT2D];
00201
00202
00203 typedef fastf_t *vect2dp_t;
00204
00205
00206 typedef fastf_t point2d_t[ELEMENTS_PER_POINT2D];
00207
00208
00209 typedef fastf_t *point2dp_t;
00210
00211
00212 typedef fastf_t vect_t[ELEMENTS_PER_VECT];
00213
00214
00215 typedef fastf_t *vectp_t;
00216
00217
00218 typedef fastf_t point_t[ELEMENTS_PER_POINT];
00219
00220
00221 typedef fastf_t *pointp_t;
00222
00223
00224 typedef fastf_t hvect_t[ELEMENTS_PER_HVECT];
00225
00226
00227 typedef hvect_t quat_t;
00228
00229
00230 typedef fastf_t hpoint_t[ELEMENTS_PER_HPOINT];
00231
00232
00233 typedef fastf_t mat_t[ELEMENTS_PER_MAT];
00234
00235
00236 typedef fastf_t *matp_t;
00237
00238
00239 typedef enum bn_vector_component_ {
00240 X = 0,
00241 Y = 1,
00242 Z = 2,
00243 H = 3,
00244 W = H
00245 } bn_vector_component;
00246
00247
00248
00249
00250
00251 typedef enum bn_matrix_component_ {
00252 MSX = 0,
00253 MDX = 3,
00254 MSY = 5,
00255 MDY = 7,
00256 MSZ = 10,
00257 MDZ = 11,
00258 MSA = 15
00259 } bn_matrix_component;
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283 typedef fastf_t plane_t[ELEMENTS_PER_PLANE];
00284
00285
00286
00287
00288
00289
00290 #define NEAR_ZERO(val, epsilon) (((val) > -epsilon) && ((val) < epsilon))
00291
00292
00293
00294
00295
00296 #define VNEAR_ZERO(v, tol) \
00297 (NEAR_ZERO(v[X], tol) \
00298 && NEAR_ZERO(v[Y], tol) \
00299 && NEAR_ZERO(v[Z], tol))
00300
00301
00302
00303
00304
00305 #define V2NEAR_ZERO(v, tol) (NEAR_ZERO(v[X], tol) && NEAR_ZERO(v[Y], tol))
00306
00307
00308
00309
00310
00311 #define HNEAR_ZERO(v, tol) \
00312 (NEAR_ZERO(v[X], tol) \
00313 && NEAR_ZERO(v[Y], tol) \
00314 && NEAR_ZERO(v[Z], tol) \
00315 && NEAR_ZERO(h[W], tol))
00316
00317
00318
00319
00320
00321
00322
00323
00324 #define ZERO(_a) NEAR_ZERO((_a), SMALL_FASTF)
00325
00326
00327
00328
00329
00330
00331
00332 #define VZERO(_a) VNEAR_ZERO((_a), SMALL_FASTF)
00333
00334
00335
00336
00337
00338
00339
00340 #define V2ZERO(_a) V2NEAR_ZERO((_a), SMALL_FASTF)
00341
00342
00343
00344
00345
00346
00347
00348 #define HZERO(_a) HNEAR_ZERO((_a), SMALL_FASTF)
00349
00350
00351
00352
00353
00354
00355 #define NEAR_EQUAL(_a, _b, _tol) NEAR_ZERO((_a) - (_b), (_tol))
00356
00357
00358
00359
00360
00361 #define VNEAR_EQUAL(_a, _b, _tol) \
00362 (NEAR_EQUAL((_a)[X], (_b)[X], (_tol)) \
00363 && NEAR_EQUAL((_a)[Y], (_b)[Y], (_tol)) \
00364 && NEAR_EQUAL((_a)[Z], (_b)[Z], (_tol)))
00365
00366
00367
00368
00369
00370 #define V2NEAR_EQUAL(a, b, tol) \
00371 (NEAR_EQUAL((a)[X], (b)[X], tol) \
00372 && NEAR_EQUAL((a)[Y], (b)[Y], tol))
00373
00374
00375
00376
00377
00378 #define HNEAR_EQUAL(_a, _b, _tol) \
00379 (NEAR_EQUAL((_a)[X], (_b)[X], (_tol)) \
00380 && NEAR_EQUAL((_a)[Y], (_b)[Y], (_tol)) \
00381 && NEAR_EQUAL((_a)[Z], (_b)[Z], (_tol)) \
00382 && NEAR_EQUAL((_a)[W], (_b)[W], (_tol)))
00383
00384
00385
00386
00387
00388
00389
00390 #define EQUAL(_a, _b) NEAR_EQUAL((_a), (_b), SMALL_FASTF)
00391
00392
00393
00394
00395
00396
00397
00398
00399 #define VEQUAL(_a, _b) VNEAR_EQUAL((_a), (_b), SMALL_FASTF)
00400
00401
00402
00403
00404
00405 #define V2EQUAL(a, b) ((a)[X]==(b)[X] && (a)[Y]==(b)[Y])
00406
00407
00408
00409
00410
00411 #define HEQUAL(a, b) ((a)[X]==(b)[X] && (a)[Y]==(b)[Y] && (a)[Z]==(b)[Z] && (a)[W]==(b)[W])
00412
00413
00414
00415
00416
00417 #define CLAMP(_v, _l, _h) if ((_v) < (_l)) _v = _l; else if ((_v) > (_h)) _v = _h
00418
00419
00420
00421 #define DIST_PT_PLANE(_pt, _pl) (VDOT(_pt, _pl) - (_pl)[W])
00422
00423
00424 #define DIST_PT_PT_SQ(_a, _b) \
00425 ((_a)[X]-(_b)[X])*((_a)[X]-(_b)[X]) + \
00426 ((_a)[Y]-(_b)[Y])*((_a)[Y]-(_b)[Y]) + \
00427 ((_a)[Z]-(_b)[Z])*((_a)[Z]-(_b)[Z])
00428 #define DIST_PT_PT(_a, _b) sqrt(DIST_PT_PT_SQ(_a, _b))
00429
00430
00431 #define MAT_DELTAS(_m, _x, _y, _z) { \
00432 (_m)[MDX] = (_x); \
00433 (_m)[MDY] = (_y); \
00434 (_m)[MDZ] = (_z); \
00435 }
00436
00437
00438 #define MAT_DELTAS_VEC(_m, _v) \
00439 MAT_DELTAS(_m, (_v)[X], (_v)[Y], (_v)[Z])
00440
00441
00442
00443
00444
00445 #define MAT_DELTAS_VEC_NEG(_m, _v) \
00446 MAT_DELTAS(_m, -(_v)[X], -(_v)[Y], -(_v)[Z])
00447
00448
00449 #define MAT_DELTAS_GET(_v, _m) { \
00450 (_v)[X] = (_m)[MDX]; \
00451 (_v)[Y] = (_m)[MDY]; \
00452 (_v)[Z] = (_m)[MDZ]; \
00453 }
00454
00455
00456
00457
00458
00459 #define MAT_DELTAS_GET_NEG(_v, _m) { \
00460 (_v)[X] = -(_m)[MDX]; \
00461 (_v)[Y] = -(_m)[MDY]; \
00462 (_v)[Z] = -(_m)[MDZ]; \
00463 }
00464
00465
00466
00467
00468
00469 #define MAT_DELTAS_ADD(_m, _x, _y, _z) { \
00470 (_m)[MDX] += (_x); \
00471 (_m)[MDY] += (_y); \
00472 (_m)[MDZ] += (_z); \
00473 }
00474
00475
00476
00477
00478
00479 #define MAT_DELTAS_ADD_VEC(_m, _v) { \
00480 (_m)[MDX] += (_v)[X]; \
00481 (_m)[MDY] += (_v)[Y]; \
00482 (_m)[MDZ] += (_v)[Z]; \
00483 }
00484
00485
00486
00487
00488
00489 #define MAT_DELTAS_SUB(_m, _x, _y, _z) { \
00490 (_m)[MDX] -= (_x); \
00491 (_m)[MDY] -= (_y); \
00492 (_m)[MDZ] -= (_z); \
00493 }
00494
00495
00496
00497
00498
00499 #define MAT_DELTAS_SUB_VEC(_m, _v) { \
00500 (_m)[MDX] -= (_v)[X]; \
00501 (_m)[MDY] -= (_v)[Y]; \
00502 (_m)[MDZ] -= (_v)[Z]; \
00503 }
00504
00505
00506
00507
00508
00509 #define MAT_DELTAS_MUL(_m, _x, _y, _z) { \
00510 (_m)[MDX] *= (_x); \
00511 (_m)[MDY] *= (_y); \
00512 (_m)[MDZ] *= (_z); \
00513 }
00514
00515
00516
00517
00518
00519 #define MAT_DELTAS_MUL_VEC(_m, _v) { \
00520 (_m)[MDX] *= (_v)[X]; \
00521 (_m)[MDY] *= (_v)[Y]; \
00522 (_m)[MDZ] *= (_v)[Z]; \
00523 }
00524
00525
00526 #define MAT_SCALE(_m, _x, _y, _z) { \
00527 (_m)[MSX] = _x; \
00528 (_m)[MSY] = _y; \
00529 (_m)[MSZ] = _z; \
00530 }
00531
00532
00533 #define MAT_SCALE_VEC(_m, _v) { \
00534 (_m)[MSX] = (_v)[X]; \
00535 (_m)[MSY] = (_v)[Y]; \
00536 (_m)[MSZ] = (_v)[Z]; \
00537 }
00538
00539
00540 #define MAT_SCALE_ALL(_m, _s) (_m)[MSA] = (_s)
00541
00542
00543 #define MAT_SCALE_ADD(_m, _x, _y, _z) { \
00544 (_m)[MSX] += _x; \
00545 (_m)[MSY] += _y; \
00546 (_m)[MSZ] += _z; \
00547 }
00548
00549
00550 #define MAT_SCALE_ADD_VEC(_m, _v) { \
00551 (_m)[MSX] += (_v)[X]; \
00552 (_m)[MSY] += (_v)[Y]; \
00553 (_m)[MSZ] += (_v)[Z]; \
00554 }
00555
00556
00557 #define MAT_SCALE_SUB(_m, _x, _y, _z) { \
00558 (_m)[MSX] -= _x; \
00559 (_m)[MSY] -= _y; \
00560 (_m)[MSZ] -= _z; \
00561 }
00562
00563
00564
00565
00566
00567 #define MAT_SCALE_SUB_VEC(_m, _v) { \
00568 (_m)[MSX] -= (_v)[X]; \
00569 (_m)[MSY] -= (_v)[Y]; \
00570 (_m)[MSZ] -= (_v)[Z]; \
00571 }
00572
00573
00574 #define MAT_SCALE_MUL(_m, _x, _y, _z) { \
00575 (_m)[MSX] *= _x; \
00576 (_m)[MSY] *= _y; \
00577 (_m)[MSZ] *= _z; \
00578 }
00579
00580
00581 #define MAT_SCALE_MUL_VEC(_m, _v) { \
00582 (_m)[MSX] *= (_v)[X]; \
00583 (_m)[MSY] *= (_v)[Y]; \
00584 (_m)[MSZ] *= (_v)[Z]; \
00585 }
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595 #define MAT_ZERO(m) { \
00596 (m)[0] = (m)[1] = (m)[2] = (m)[3] = \
00597 (m)[4] = (m)[5] = (m)[6] = (m)[7] = \
00598 (m)[8] = (m)[9] = (m)[10] = (m)[11] = \
00599 (m)[12] = (m)[13] = (m)[14] = (m)[15] = 0.0; \
00600 }
00601
00602
00603 #define MAT_IDN(m) { \
00604 (m)[1] = (m)[2] = (m)[3] = (m)[4] = \
00605 (m)[6] = (m)[7] = (m)[8] = (m)[9] = \
00606 (m)[11] = (m)[12] = (m)[13] = (m)[14] = 0.0; \
00607 (m)[0] = (m)[5] = (m)[10] = (m)[15] = 1.0; \
00608 }
00609
00610
00611 #define MAT_TRANSPOSE(t, m) { \
00612 (t)[0] = (m)[0]; \
00613 (t)[4] = (m)[1]; \
00614 (t)[8] = (m)[2]; \
00615 (t)[12] = (m)[3]; \
00616 (t)[1] = (m)[4]; \
00617 (t)[5] = (m)[5]; \
00618 (t)[9] = (m)[6]; \
00619 (t)[13] = (m)[7]; \
00620 (t)[2] = (m)[8]; \
00621 (t)[6] = (m)[9]; \
00622 (t)[10] = (m)[10]; \
00623 (t)[14] = (m)[11]; \
00624 (t)[3] = (m)[12]; \
00625 (t)[7] = (m)[13]; \
00626 (t)[11] = (m)[14]; \
00627 (t)[15] = (m)[15]; \
00628 }
00629
00630
00631 #define MAT_COPY(d, s) { \
00632 (d)[0] = (s)[0]; \
00633 (d)[1] = (s)[1]; \
00634 (d)[2] = (s)[2]; \
00635 (d)[3] = (s)[3]; \
00636 (d)[4] = (s)[4]; \
00637 (d)[5] = (s)[5]; \
00638 (d)[6] = (s)[6]; \
00639 (d)[7] = (s)[7]; \
00640 (d)[8] = (s)[8]; \
00641 (d)[9] = (s)[9]; \
00642 (d)[10] = (s)[10]; \
00643 (d)[11] = (s)[11]; \
00644 (d)[12] = (s)[12]; \
00645 (d)[13] = (s)[13]; \
00646 (d)[14] = (s)[14]; \
00647 (d)[15] = (s)[15]; \
00648 }
00649
00650
00651 #define VSET(a, b, c, d) { \
00652 (a)[X] = (b); \
00653 (a)[Y] = (c); \
00654 (a)[Z] = (d); \
00655 }
00656
00657
00658 #define V2SET(a, b, c) { \
00659 (a)[X] = (b); \
00660 (a)[Y] = (c); \
00661 }
00662
00663
00664 #define HSET(a, b, c, d, e) { \
00665 (a)[X] = (b); \
00666 (a)[Y] = (c); \
00667 (a)[Z] = (d); \
00668 (a)[H] = (e); \
00669 }
00670
00671
00672
00673 #define VSETALL(a, s) { \
00674 (a)[X] = (a)[Y] = (a)[Z] = (s); \
00675 }
00676
00677
00678 #define V2SETALL(a, s) { \
00679 (a)[X] = (a)[Y] = (s); \
00680 }
00681
00682
00683 #define HSETALL(a, s) { \
00684 (a)[X] = (a)[Y] = (a)[Z] = (a)[H] = (s); \
00685 }
00686
00687
00688
00689 #define VSETALLN(v, s, n) { \
00690 register int _j; \
00691 for (_j=0; _j<n; _j++) v[_j]=(s); \
00692 }
00693
00694
00695
00696 #define VMOVE(a, b) { \
00697 (a)[X] = (b)[X]; \
00698 (a)[Y] = (b)[Y]; \
00699 (a)[Z] = (b)[Z]; \
00700 }
00701
00702
00703 #define V2MOVE(a, b) { \
00704 (a)[X] = (b)[X]; \
00705 (a)[Y] = (b)[Y]; \
00706 }
00707
00708
00709 #define HMOVE(a, b) { \
00710 (a)[X] = (b)[X]; \
00711 (a)[Y] = (b)[Y]; \
00712 (a)[Z] = (b)[Z]; \
00713 (a)[W] = (b)[W]; \
00714 }
00715
00716
00717 #define VMOVEN(a, b, n) { \
00718 register int _vmove; \
00719 for (_vmove = 0; _vmove < (n); _vmove++) { \
00720 (a)[_vmove] = (b)[_vmove]; \
00721 } \
00722 }
00723
00724
00725
00726 #define VREVERSE(a, b) { \
00727 (a)[X] = -(b)[X]; \
00728 (a)[Y] = -(b)[Y]; \
00729 (a)[Z] = -(b)[Z]; \
00730 }
00731
00732
00733 #define V2REVERSE(a, b) { \
00734 (a)[X] = -(b)[X]; \
00735 (a)[Y] = -(b)[Y]; \
00736 }
00737
00738
00739
00740
00741
00742 #define HREVERSE(a, b) { \
00743 (a)[X] = -(b)[X]; \
00744 (a)[Y] = -(b)[Y]; \
00745 (a)[Z] = -(b)[Z]; \
00746 (a)[W] = -(b)[W]; \
00747 }
00748
00749
00750 #define VADD2(a, b, c) { \
00751 (a)[X] = (b)[X] + (c)[X]; \
00752 (a)[Y] = (b)[Y] + (c)[Y]; \
00753 (a)[Z] = (b)[Z] + (c)[Z]; \
00754 }
00755
00756
00757 #define V2ADD2(a, b, c) { \
00758 (a)[X] = (b)[X] + (c)[X]; \
00759 (a)[Y] = (b)[Y] + (c)[Y]; \
00760 }
00761
00762
00763 #define HADD2(a, b, c) { \
00764 (a)[X] = (b)[X] + (c)[X]; \
00765 (a)[Y] = (b)[Y] + (c)[Y]; \
00766 (a)[Z] = (b)[Z] + (c)[Z]; \
00767 (a)[W] = (b)[W] + (c)[W]; \
00768 }
00769
00770
00771
00772
00773
00774 #define VADD2N(a, b, c, n) { \
00775 register int _vadd2; \
00776 for (_vadd2 = 0; _vadd2 < (n); _vadd2++) { \
00777 (a)[_vadd2] = (b)[_vadd2] + (c)[_vadd2]; \
00778 } \
00779 }
00780
00781
00782
00783
00784
00785
00786 #define VSUB2(a, b, c) { \
00787 (a)[X] = (b)[X] - (c)[X]; \
00788 (a)[Y] = (b)[Y] - (c)[Y]; \
00789 (a)[Z] = (b)[Z] - (c)[Z]; \
00790 }
00791
00792
00793
00794
00795
00796 #define V2SUB2(a, b, c) { \
00797 (a)[X] = (b)[X] - (c)[X]; \
00798 (a)[Y] = (b)[Y] - (c)[Y]; \
00799 }
00800
00801
00802
00803
00804
00805 #define HSUB2(a, b, c) { \
00806 (a)[X] = (b)[X] - (c)[X]; \
00807 (a)[Y] = (b)[Y] - (c)[Y]; \
00808 (a)[Z] = (b)[Z] - (c)[Z]; \
00809 (a)[W] = (b)[W] - (c)[W]; \
00810 }
00811
00812
00813
00814
00815
00816 #define VSUB2N(a, b, c, n) { \
00817 register int _vsub2; \
00818 for (_vsub2 = 0; _vsub2 < (n); _vsub2++) { \
00819 (a)[_vsub2] = (b)[_vsub2] - (c)[_vsub2]; \
00820 } \
00821 }
00822
00823
00824
00825 #define VSUB3(a, b, c, d) { \
00826 (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00827 (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00828 (a)[Z] = (b)[Z] - (c)[Z] - (d)[Z]; \
00829 }
00830
00831
00832 #define V2SUB3(a, b, c, d) { \
00833 (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00834 (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00835 }
00836
00837
00838 #define HSUB3(a, b, c, d) { \
00839 (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00840 (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00841 (a)[Z] = (b)[Z] - (c)[Z] - (d)[Z]; \
00842 (a)[W] = (b)[W] - (c)[W] - (d)[W]; \
00843 }
00844
00845
00846 #define VSUB3N(a, b, c, d, n) { \
00847 register int _vsub3; \
00848 for (_vsub3 = 0; _vsub3 < (n); _vsub3++) { \
00849 (a)[_vsub3] = (b)[_vsub3] - (c)[_vsub3] - (d)[_vsub3]; \
00850 } \
00851 }
00852
00853
00854
00855 #define VADD3(a, b, c, d) { \
00856 (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00857 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00858 (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z]; \
00859 }
00860
00861
00862 #define V2ADD3(a, b, c, d) { \
00863 (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00864 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00865 }
00866
00867
00868 #define HADD3(a, b, c, d) { \
00869 (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00870 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00871 (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z]; \
00872 (a)[W] = (b)[W] + (c)[W] + (d)[W]; \
00873 }
00874
00875
00876
00877
00878
00879 #define VADD3N(a, b, c, d, n) { \
00880 register int _vadd3; \
00881 for (_vadd3 = 0; _vadd3 < (n); _vadd3++) { \
00882 (a)[_vadd3] = (b)[_vadd3] + (c)[_vadd3] + (d)[_vadd3]; \
00883 } \
00884 }
00885
00886
00887
00888
00889
00890
00891 #define VADD4(a, b, c, d, e) { \
00892 (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00893 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00894 (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z] + (e)[Z]; \
00895 }
00896
00897
00898
00899
00900
00901 #define V2ADD4(a, b, c, d, e) { \
00902 (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00903 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00904 }
00905
00906
00907
00908
00909
00910 #define HADD4(a, b, c, d, e) { \
00911 (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00912 (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00913 (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z] + (e)[Z]; \
00914 (a)[W] = (b)[W] + (c)[W] + (d)[W] + (e)[W]; \
00915 }
00916
00917
00918
00919
00920
00921 #define VADD4N(a, b, c, d, e, n) { \
00922 register int _vadd4; \
00923 for (_vadd4 = 0; _vadd4 < (n); _vadd4++) { \
00924 (a)[_vadd4] = (b)[_vadd4] + (c)[_vadd4] + (d)[_vadd4] + (e)[_vadd4]; \
00925 } \
00926 }
00927
00928
00929
00930 #define VSCALE(a, b, c) { \
00931 (a)[X] = (b)[X] * (c); \
00932 (a)[Y] = (b)[Y] * (c); \
00933 (a)[Z] = (b)[Z] * (c); \
00934 }
00935
00936
00937 #define V2SCALE(a, b, c) { \
00938 (a)[X] = (b)[X] * (c); \
00939 (a)[Y] = (b)[Y] * (c); \
00940 }
00941
00942
00943 #define HSCALE(a, b, c) { \
00944 (a)[X] = (b)[X] * (c); \
00945 (a)[Y] = (b)[Y] * (c); \
00946 (a)[Z] = (b)[Z] * (c); \
00947 (a)[W] = (b)[W] * (c); \
00948 }
00949
00950
00951
00952
00953
00954 #define VSCALEN(a, b, c, n) { \
00955 register int _vscale; \
00956 for (_vscale = 0; _vscale < (n); _vscale++) { \
00957 (a)[_vscale] = (b)[_vscale] * (c); \
00958 } \
00959 }
00960
00961
00962 #define VUNITIZE(a) { \
00963 register double _f = MAGSQ(a); \
00964 if (! NEAR_EQUAL(_f, 1.0, VUNITIZE_TOL)) { \
00965 _f = sqrt(_f); \
00966 if (_f < VDIVIDE_TOL) { \
00967 VSETALL((a), 0.0); \
00968 } else { \
00969 _f = 1.0/_f; \
00970 (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; \
00971 } \
00972 } \
00973 }
00974
00975
00976 #define VUNITIZE_RET(a, ret) { \
00977 register double _f; \
00978 _f = MAGNITUDE(a); \
00979 if (_f < VDIVIDE_TOL) return ret; \
00980 _f = 1.0/_f; \
00981 (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; \
00982 }
00983
00984
00985
00986
00987
00988 #define VADD2SCALE(o, a, b, s) { \
00989 (o)[X] = ((a)[X] + (b)[X]) * (s); \
00990 (o)[Y] = ((a)[Y] + (b)[Y]) * (s); \
00991 (o)[Z] = ((a)[Z] + (b)[Z]) * (s); \
00992 }
00993
00994 #define VADD2SCALEN(o, a, b, n) { \
00995 register int _vadd2scale; \
00996 for (_vadd2scale = 0; \
00997 _vadd2scale < (n); \
00998 _vadd2scale++) { \
00999 (o)[_vadd2scale] = ((a)[_vadd2scale] + (b)[_vadd2scale]) * (s); \
01000 } \
01001 }
01002
01003
01004
01005
01006
01007 #define VSUB2SCALE(o, a, b, s) { \
01008 (o)[X] = ((a)[X] - (b)[X]) * (s); \
01009 (o)[Y] = ((a)[Y] - (b)[Y]) * (s); \
01010 (o)[Z] = ((a)[Z] - (b)[Z]) * (s); \
01011 }
01012
01013 #define VSUB2SCALEN(o, a, b, n) { \
01014 register int _vsub2scale; \
01015 for (_vsub2scale = 0; \
01016 _vsub2scale < (n); \
01017 _vsub2scale++) { \
01018 (o)[_vsub2scale] = ((a)[_vsub2scale] - (b)[_vsub2scale]) * (s); \
01019 } \
01020 }
01021
01022
01023
01024 #define VCOMB3(o, a, b, c, d, e, f) { \
01025 (o)[X] = (a) * (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01026 (o)[Y] = (a) * (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01027 (o)[Z] = (a) * (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01028 }
01029
01030 #define VCOMB3N(o, a, b, c, d, e, f, n) { \
01031 register int _vcomb3; \
01032 for (_vcomb3 = 0; \
01033 _vcomb3 < (n); \
01034 _vcomb3++) { \
01035 (o)[_vcomb3] = (a) * (b)[_vcomb3] + (c) * (d)[_vcomb3] + (e) * (f)[_vcomb3]; \
01036 } \
01037 }
01038
01039 #define VCOMB2(o, a, b, c, d) { \
01040 (o)[X] = (a) * (b)[X] + (c) * (d)[X]; \
01041 (o)[Y] = (a) * (b)[Y] + (c) * (d)[Y]; \
01042 (o)[Z] = (a) * (b)[Z] + (c) * (d)[Z]; \
01043 }
01044
01045 #define VCOMB2N(o, a, b, c, d, n) { \
01046 register int _vcomb2; \
01047 for (_vcomb2 = 0; \
01048 _vcomb2 < (n); \
01049 _vcomb2++) { \
01050 (o)[_vcomb2] = (a) * (b)[_vcomb2] + (c) * (d)[_vcomb2]; \
01051 } \
01052 }
01053
01054 #define VJOIN4(a, b, c, d, e, f, g, h, i, j) { \
01055 (a)[X] = (b)[X] + (c)*(d)[X] + (e)*(f)[X] + (g)*(h)[X] + (i)*(j)[X]; \
01056 (a)[Y] = (b)[Y] + (c)*(d)[Y] + (e)*(f)[Y] + (g)*(h)[Y] + (i)*(j)[Y]; \
01057 (a)[Z] = (b)[Z] + (c)*(d)[Z] + (e)*(f)[Z] + (g)*(h)[Z] + (i)*(j)[Z]; \
01058 }
01059
01060 #define VJOIN3(a, b, c, d, e, f, g, h) { \
01061 (a)[X] = (b)[X] + (c)*(d)[X] + (e)*(f)[X] + (g)*(h)[X]; \
01062 (a)[Y] = (b)[Y] + (c)*(d)[Y] + (e)*(f)[Y] + (g)*(h)[Y]; \
01063 (a)[Z] = (b)[Z] + (c)*(d)[Z] + (e)*(f)[Z] + (g)*(h)[Z]; \
01064 }
01065
01066
01067
01068
01069
01070
01071
01072
01073 #define VJOIN2(a, b, c, d, e, f) { \
01074 (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01075 (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01076 (a)[Z] = (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01077 }
01078
01079
01080
01081
01082
01083
01084
01085 #define V2JOIN2(a, b, c, d, e, f) { \
01086 (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01087 (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01088 }
01089
01090
01091
01092
01093
01094
01095
01096 #define HJOIN2(a, b, c, d, e, f) { \
01097 (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01098 (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01099 (a)[Z] = (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01100 (a)[W] = (b)[W] + (c) * (d)[W] + (e) * (f)[W]; \
01101 }
01102
01103 #define VJOIN2N(a, b, c, d, e, f, n) { \
01104 register int _vjoin2; \
01105 for (_vjoin2 = 0; \
01106 _vjoin2 < (n); \
01107 _vjoin2++) { \
01108 (a)[_vjoin2] = (b)[_vjoin2] + (c) * (d)[_vjoin2] + (e) * (f)[_vjoin2]; \
01109 } \
01110 }
01111
01112
01113 #define VJOIN1(a, b, c, d) { \
01114 (a)[X] = (b)[X] + (c) * (d)[X]; \
01115 (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01116 (a)[Z] = (b)[Z] + (c) * (d)[Z]; \
01117 }
01118
01119 #define V2JOIN1(a, b, c, d) { \
01120 (a)[X] = (b)[X] + (c) * (d)[X]; \
01121 (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01122 }
01123
01124 #define HJOIN1(a, b, c, d) { \
01125 (a)[X] = (b)[X] + (c) * (d)[X]; \
01126 (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01127 (a)[Z] = (b)[Z] + (c) * (d)[Z]; \
01128 (a)[W] = (b)[W] + (c) * (d)[W]; \
01129 }
01130
01131 #define VJOIN1N(a, b, c, d, n) { \
01132 register int _vjoin1; \
01133 for (_vjoin1 = 0; \
01134 _vjoin1 < (n); \
01135 _vjoin1++) { \
01136 (a)[_vjoin1] = (b)[_vjoin1] + (c) * (d)[_vjoin1]; \
01137 } \
01138 }
01139
01140
01141
01142
01143
01144
01145
01146 #define VBLEND2(a, b, c, d, e) { \
01147 (a)[X] = (b) * (c)[X] + (d) * (e)[X]; \
01148 (a)[Y] = (b) * (c)[Y] + (d) * (e)[Y]; \
01149 (a)[Z] = (b) * (c)[Z] + (d) * (e)[Z]; \
01150 }
01151
01152 #define VBLEND2N(a, b, c, d, e, n) { \
01153 register int _vblend2; \
01154 for (_vblend2 = 0; \
01155 _vblend2 < (n); \
01156 _vblend2++) { \
01157 (a)[_vblend2] = (b) * (c)[_vblend2] + (d) * (e)[_vblend2]; \
01158 } \
01159 }
01160
01161
01162
01163
01164
01165
01166
01167 #define VPROJECT(a, b, c, d) { \
01168 VSCALE(c, b, VDOT(a, b) / VDOT(b, b)); \
01169 VSUB2(d, a, c); \
01170 }
01171
01172
01173 #define MAGSQ(a) ((a)[X]*(a)[X] + (a)[Y]*(a)[Y] + (a)[Z]*(a)[Z])
01174 #define MAG2SQ(a) ((a)[X]*(a)[X] + (a)[Y]*(a)[Y])
01175
01176
01177 #define MAGNITUDE(a) sqrt(MAGSQ(a))
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193 #define VCROSS(a, b, c) { \
01194 (a)[X] = (b)[Y] * (c)[Z] - (b)[Z] * (c)[Y]; \
01195 (a)[Y] = (b)[Z] * (c)[X] - (b)[X] * (c)[Z]; \
01196 (a)[Z] = (b)[X] * (c)[Y] - (b)[Y] * (c)[X]; \
01197 }
01198
01199
01200 #define VDOT(a, b) ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] + (a)[Z]*(b)[Z])
01201
01202 #define V2DOT(a, b) ((a)[X]*(b)[X] + (a)[Y]*(b)[Y])
01203
01204 #define HDOT(a, b) ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] + (a)[Z]*(b)[Z] + (a)[W]*(b)[W])
01205
01206
01207
01208
01209
01210
01211 #define VSUB2DOT(_pt2, _pt, _vec) (\
01212 ((_pt2)[X] - (_pt)[X]) * (_vec)[X] + \
01213 ((_pt2)[Y] - (_pt)[Y]) * (_vec)[Y] + \
01214 ((_pt2)[Z] - (_pt)[Z]) * (_vec)[Z])
01215
01216
01217
01218
01219
01220 #define V2ARGS(a) (a)[X], (a)[Y]
01221 #define V3ARGS(a) (a)[X], (a)[Y], (a)[Z]
01222 #define V4ARGS(a) (a)[X], (a)[Y], (a)[Z], (a)[W]
01223
01224
01225
01226
01227
01228
01229
01230
01231
01232 #define INTCLAMP(_a) (NEAR_EQUAL((_a), rint(_a), VUNITIZE_TOL) ? (double)(long)rint(_a) : (_a))
01233
01234
01235 #define V2INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y])
01236
01237 #define V3INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y]), INTCLAMP((a)[Z])
01238
01239 #define V4INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y]), INTCLAMP((a)[Z]), INTCLAMP((a)[W])
01240
01241
01242 #define V2PRINT(a, b) \
01243 (void)fprintf(stderr, "%s (%g, %g)\n", a, V2ARGS(b));
01244 #define VPRINT(a, b) \
01245 (void)fprintf(stderr, "%s (%g, %g, %g)\n", a, V3ARGS(b));
01246 #define HPRINT(a, b) \
01247 (void)fprintf(stderr, "%s (%g, %g, %g, %g)\n", a, V4ARGS(b));
01248
01249
01250
01251
01252
01253
01254 #define V2INTCLAMPPRINT(a, b) \
01255 (void)fprintf(stderr, "%s (%g, %g)\n", a, V2INTCLAMPARGS(b));
01256 #define VINTCLAMPPRINT(a, b) \
01257 (void)fprintf(stderr, "%s (%g, %g, %g)\n", a, V3INTCLAMPARGS(b));
01258 #define HINTCLAMPPRINT(a, b) \
01259 (void)fprintf(stderr, "%s (%g, %g, %g, %g)\n", a, V4INTCLAMPARGS(b));
01260
01261 #ifdef __cplusplus
01262 #define CPP_V3PRINT(_os, _title, _p) (_os) << (_title) << "=(" << \
01263 (_p)[X] << ", " << (_p)[Y] << ")\n";
01264 #define CPP_VPRINT(_os, _title, _p) (_os) << (_title) << "=(" << \
01265 (_p)[X] << ", " << (_p)[Y] << ", " << (_p)[Z] << ")\n";
01266 #define CPP_HPRINT(_os, _title, _p) (_os) << (_title) << "=(" << \
01267 (_p)[X] << ", " << (_p)[Y] << ", " << (_p)[Z] << ", " << (_p)[W]<< ")\n";
01268 #endif
01269
01270
01271 #define VELMUL(a, b, c) { \
01272 (a)[X] = (b)[X] * (c)[X]; \
01273 (a)[Y] = (b)[Y] * (c)[Y]; \
01274 (a)[Z] = (b)[Z] * (c)[Z]; \
01275 }
01276
01277 #define VELMUL3(a, b, c, d) { \
01278 (a)[X] = (b)[X] * (c)[X] * (d)[X]; \
01279 (a)[Y] = (b)[Y] * (c)[Y] * (d)[Y]; \
01280 (a)[Z] = (b)[Z] * (c)[Z] * (d)[Z]; \
01281 }
01282
01283
01284 #define VELDIV(a, b, c) { \
01285 (a)[0] = (b)[0] / (c)[0]; \
01286 (a)[1] = (b)[1] / (c)[1]; \
01287 (a)[2] = (b)[2] / (c)[2]; \
01288 }
01289
01290
01291
01292
01293
01294 #define VINVDIR(_inv, _dir) { \
01295 if ((_dir)[X] < -SQRT_SMALL_FASTF || (_dir)[X] > SQRT_SMALL_FASTF) { \
01296 (_inv)[X]=1.0/(_dir)[X]; \
01297 } else { \
01298 (_dir)[X] = 0.0; \
01299 (_inv)[X] = INFINITY; \
01300 } \
01301 if ((_dir)[Y] < -SQRT_SMALL_FASTF || (_dir)[Y] > SQRT_SMALL_FASTF) { \
01302 (_inv)[Y]=1.0/(_dir)[Y]; \
01303 } else { \
01304 (_dir)[Y] = 0.0; \
01305 (_inv)[Y] = INFINITY; \
01306 } \
01307 if ((_dir)[Z] < -SQRT_SMALL_FASTF || (_dir)[Z] > SQRT_SMALL_FASTF) { \
01308 (_inv)[Z]=1.0/(_dir)[Z]; \
01309 } else { \
01310 (_dir)[Z] = 0.0; \
01311 (_inv)[Z] = INFINITY; \
01312 } \
01313 }
01314
01315
01316
01317
01318
01319 #define MAT3X3VEC(o, mat, vec) { \
01320 (o)[X] = (mat)[X]*(vec)[X]+(mat)[Y]*(vec)[Y] + (mat)[ 2]*(vec)[Z]; \
01321 (o)[Y] = (mat)[4]*(vec)[X]+(mat)[5]*(vec)[Y] + (mat)[ 6]*(vec)[Z]; \
01322 (o)[Z] = (mat)[8]*(vec)[X]+(mat)[9]*(vec)[Y] + (mat)[10]*(vec)[Z]; \
01323 }
01324
01325
01326 #define VEC3X3MAT(o, i, m) { \
01327 (o)[X] = (i)[X]*(m)[X] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8]; \
01328 (o)[Y] = (i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9]; \
01329 (o)[Z] = (i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10]; \
01330 }
01331
01332
01333 #define MAT3X2VEC(o, mat, vec) { \
01334 (o)[X] = (mat)[0]*(vec)[X] + (mat)[Y]*(vec)[Y]; \
01335 (o)[Y] = (mat)[4]*(vec)[X] + (mat)[5]*(vec)[Y]; \
01336 (o)[Z] = (mat)[8]*(vec)[X] + (mat)[9]*(vec)[Y]; \
01337 }
01338
01339
01340 #define VEC2X3MAT(o, i, m) { \
01341 (o)[X] = (i)[X]*(m)[0] + (i)[Y]*(m)[4]; \
01342 (o)[Y] = (i)[X]*(m)[1] + (i)[Y]*(m)[5]; \
01343 (o)[Z] = (i)[X]*(m)[2] + (i)[Y]*(m)[6]; \
01344 }
01345
01346
01347
01348
01349
01350 #define MAT4X3PNT(o, m, i) { \
01351 register double _f; \
01352 _f = 1.0/((m)[12]*(i)[X] + (m)[13]*(i)[Y] + (m)[14]*(i)[Z] + (m)[15]); \
01353 (o)[X]=((m)[0]*(i)[X] + (m)[1]*(i)[Y] + (m)[ 2]*(i)[Z] + (m)[3]) * _f; \
01354 (o)[Y]=((m)[4]*(i)[X] + (m)[5]*(i)[Y] + (m)[ 6]*(i)[Z] + (m)[7]) * _f; \
01355 (o)[Z]=((m)[8]*(i)[X] + (m)[9]*(i)[Y] + (m)[10]*(i)[Z] + (m)[11])* _f; \
01356 }
01357
01358
01359
01360
01361
01362 #define PNT3X4MAT(o, i, m) { \
01363 register double _f; \
01364 _f = 1.0/((i)[X]*(m)[3] + (i)[Y]*(m)[7] + (i)[Z]*(m)[11] + (m)[15]); \
01365 (o)[X]=((i)[X]*(m)[0] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8] + (m)[12]) * _f; \
01366 (o)[Y]=((i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9] + (m)[13]) * _f; \
01367 (o)[Z]=((i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10] + (m)[14])* _f; \
01368 }
01369
01370
01371
01372
01373
01374 #define MAT4X4PNT(o, m, i) { \
01375 (o)[X]=(m)[ 0]*(i)[X] + (m)[ 1]*(i)[Y] + (m)[ 2]*(i)[Z] + (m)[ 3]*(i)[H]; \
01376 (o)[Y]=(m)[ 4]*(i)[X] + (m)[ 5]*(i)[Y] + (m)[ 6]*(i)[Z] + (m)[ 7]*(i)[H]; \
01377 (o)[Z]=(m)[ 8]*(i)[X] + (m)[ 9]*(i)[Y] + (m)[10]*(i)[Z] + (m)[11]*(i)[H]; \
01378 (o)[H]=(m)[12]*(i)[X] + (m)[13]*(i)[Y] + (m)[14]*(i)[Z] + (m)[15]*(i)[H]; \
01379 }
01380
01381
01382
01383
01384
01385
01386 #define MAT4X3VEC(o, m, i) { \
01387 register double _f; \
01388 _f = 1.0/((m)[15]); \
01389 (o)[X] = ((m)[0]*(i)[X] + (m)[1]*(i)[Y] + (m)[ 2]*(i)[Z]) * _f; \
01390 (o)[Y] = ((m)[4]*(i)[X] + (m)[5]*(i)[Y] + (m)[ 6]*(i)[Z]) * _f; \
01391 (o)[Z] = ((m)[8]*(i)[X] + (m)[9]*(i)[Y] + (m)[10]*(i)[Z]) * _f; \
01392 }
01393
01394 #define MAT4XSCALOR(o, m, i) { \
01395 (o) = (i) / (m)[15]; \
01396 }
01397
01398
01399
01400
01401
01402 #define VEC3X4MAT(o, i, m) { \
01403 register double _f; \
01404 _f = 1.0/((m)[15]); \
01405 (o)[X] = ((i)[X]*(m)[0] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8]) * _f; \
01406 (o)[Y] = ((i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9]) * _f; \
01407 (o)[Z] = ((i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10]) * _f; \
01408 }
01409
01410
01411 #define VEC2X4MAT(o, i, m) { \
01412 register double _f; \
01413 _f = 1.0/((m)[15]); \
01414 (o)[X] = ((i)[X]*(m)[0] + (i)[Y]*(m)[4]) * _f; \
01415 (o)[Y] = ((i)[X]*(m)[1] + (i)[Y]*(m)[5]) * _f; \
01416 (o)[Z] = ((i)[X]*(m)[2] + (i)[Y]*(m)[6]) * _f; \
01417 }
01418
01419
01420 #define BN_VEC_NON_UNIT_LEN(_vec) \
01421 (fabs(MAGSQ(_vec)) < 0.0001 || fabs(fabs(MAGSQ(_vec))-1) > 0.0001)
01422
01423
01424
01425
01426
01427
01428 #define V_MIN(r, s) if ((r) > (s)) r = (s)
01429
01430 #define V_MAX(r, s) if ((r) < (s)) r = (s)
01431
01432 #define VMIN(r, s) { \
01433 V_MIN((r)[X], (s)[X]); V_MIN((r)[Y], (s)[Y]); V_MIN((r)[Z], (s)[Z]); \
01434 }
01435
01436 #define VMAX(r, s) { \
01437 V_MAX((r)[X], (s)[X]); V_MAX((r)[Y], (s)[Y]); V_MAX((r)[Z], (s)[Z]); \
01438 }
01439
01440 #define VMINMAX(min, max, pt) { \
01441 VMIN((min), (pt)); VMAX((max), (pt)); \
01442 }
01443
01444
01445
01446
01447
01448 #define HDIVIDE(a, b) { \
01449 (a)[X] = (b)[X] / (b)[H]; \
01450 (a)[Y] = (b)[Y] / (b)[H]; \
01451 (a)[Z] = (b)[Z] / (b)[H]; \
01452 }
01453
01454
01455
01456
01457
01458
01459
01460
01461
01462
01463
01464 #define VADD2_2D(a, b, c) V2ADD2(a, b, c)
01465 #define VSUB2_2D(a, b, c) V2SUB2(a, b, c)
01466 #define MAGSQ_2D(a) MAG2SQ(a)
01467 #define VDOT_2D(a, b) V2DOT(a, b)
01468 #define VMOVE_2D(a, b) V2MOVE(a, b)
01469 #define VSCALE_2D(a, b, c) V2SCALE(a, b, c)
01470 #define VJOIN1_2D(a, b, c, d) V2JOIN1(a, b, c, d)
01471
01472
01473
01474
01475
01476
01477
01478
01479
01480
01481
01482
01483
01484
01485
01486
01487
01488
01489
01490
01491
01492
01493
01494
01495
01496 #define QUAT_FROM_ROT(q, r, x, y, z) { \
01497 register fastf_t _rot = (r) * 0.5; \
01498 QSET(q, x, y, z, cos(_rot)); \
01499 VUNITIZE(q); \
01500 _rot = sin(_rot); \
01501 VSCALE(q, q, _rot); \
01502 }
01503
01504 #define QUAT_FROM_VROT(q, r, v) { \
01505 register fastf_t _rot = (r) * 0.5; \
01506 VMOVE(q, v); \
01507 VUNITIZE(q); \
01508 (q)[W] = cos(_rot); \
01509 _rot = sin(_rot); \
01510 VSCALE(q, q, _rot); \
01511 }
01512
01513 #define QUAT_FROM_VROT_DEG(q, r, v) \
01514 QUAT_FROM_VROT(q, ((r)*(M_PI/180.0)), v)
01515
01516 #define QUAT_FROM_ROT_DEG(q, r, x, y, z) \
01517 QUAT_FROM_ROT(q, ((r)*(M_PI/180.0)), x, y, z)
01518
01519
01520
01521
01522
01523
01524 #define QSET(a, b, c, d, e) { \
01525 (a)[X] = (b); \
01526 (a)[Y] = (c); \
01527 (a)[Z] = (d); \
01528 (a)[W] = (e); \
01529 }
01530
01531
01532 #define QMOVE(a, b) { \
01533 (a)[X] = (b)[X]; \
01534 (a)[Y] = (b)[Y]; \
01535 (a)[Z] = (b)[Z]; \
01536 (a)[W] = (b)[W]; \
01537 }
01538
01539
01540 #define QADD2(a, b, c) { \
01541 (a)[X] = (b)[X] + (c)[X]; \
01542 (a)[Y] = (b)[Y] + (c)[Y]; \
01543 (a)[Z] = (b)[Z] + (c)[Z]; \
01544 (a)[W] = (b)[W] + (c)[W]; \
01545 }
01546
01547
01548
01549
01550
01551 #define QSUB2(a, b, c) { \
01552 (a)[X] = (b)[X] - (c)[X]; \
01553 (a)[Y] = (b)[Y] - (c)[Y]; \
01554 (a)[Z] = (b)[Z] - (c)[Z]; \
01555 (a)[W] = (b)[W] - (c)[W]; \
01556 }
01557
01558
01559
01560
01561
01562 #define QSCALE(a, b, c) { \
01563 (a)[X] = (b)[X] * (c); \
01564 (a)[Y] = (b)[Y] * (c); \
01565 (a)[Z] = (b)[Z] * (c); \
01566 (a)[W] = (b)[W] * (c); \
01567 }
01568
01569
01570 #define QUNITIZE(a) { \
01571 register double _f; \
01572 _f = QMAGNITUDE(a); \
01573 if (_f < VDIVIDE_TOL) _f = 0.0; else _f = 1.0/_f; \
01574 (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; (a)[W] *= _f; \
01575 }
01576
01577
01578 #define QMAGSQ(a) \
01579 ((a)[X]*(a)[X] + (a)[Y]*(a)[Y] \
01580 + (a)[Z]*(a)[Z] + (a)[W]*(a)[W])
01581
01582
01583 #define QMAGNITUDE(a) sqrt(QMAGSQ(a))
01584
01585
01586 #define QDOT(a, b) \
01587 ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] \
01588 + (a)[Z]*(b)[Z] + (a)[W]*(b)[W])
01589
01590
01591
01592
01593
01594
01595
01596
01597 #define QMUL(a, b, c) { \
01598 (a)[W] = (b)[W]*(c)[W] - (b)[X]*(c)[X] - (b)[Y]*(c)[Y] - (b)[Z]*(c)[Z]; \
01599 (a)[X] = (b)[W]*(c)[X] + (b)[X]*(c)[W] + (b)[Y]*(c)[Z] - (b)[Z]*(c)[Y]; \
01600 (a)[Y] = (b)[W]*(c)[Y] + (b)[Y]*(c)[W] + (b)[Z]*(c)[X] - (b)[X]*(c)[Z]; \
01601 (a)[Z] = (b)[W]*(c)[Z] + (b)[Z]*(c)[W] + (b)[X]*(c)[Y] - (b)[Y]*(c)[X]; \
01602 }
01603
01604
01605 #define QCONJUGATE(a, b) { \
01606 (a)[X] = -(b)[X]; \
01607 (a)[Y] = -(b)[Y]; \
01608 (a)[Z] = -(b)[Z]; \
01609 (a)[W] = (b)[W]; \
01610 }
01611
01612
01613 #define QINVERSE(a, b) { \
01614 register double _f = QMAGSQ(b); \
01615 if (_f < VDIVIDE_TOL) _f = 0.0; else _f = 1.0/_f; \
01616 (a)[X] = -(b)[X] * _f; \
01617 (a)[Y] = -(b)[Y] * _f; \
01618 (a)[Z] = -(b)[Z] * _f; \
01619 (a)[W] = (b)[W] * _f; \
01620 }
01621
01622
01623
01624
01625
01626
01627
01628 #define QBLEND2(a, b, c, d, e) { \
01629 (a)[X] = (b) * (c)[X] + (d) * (e)[X]; \
01630 (a)[Y] = (b) * (c)[Y] + (d) * (e)[Y]; \
01631 (a)[Z] = (b) * (c)[Z] + (d) * (e)[Z]; \
01632 (a)[W] = (b) * (c)[W] + (d) * (e)[W]; \
01633 }
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645 #define V3RPP_DISJOINT(_l1, _h1, _l2, _h2) \
01646 ((_l1)[X] > (_h2)[X] || (_l1)[Y] > (_h2)[Y] || (_l1)[Z] > (_h2)[Z] || \
01647 (_l2)[X] > (_h1)[X] || (_l2)[Y] > (_h1)[Y] || (_l2)[Z] > (_h1)[Z])
01648
01649
01650
01651
01652
01653 #define V3RPP_DISJOINT_TOL(_l1, _h1, _l2, _h2, _t) \
01654 (((_l1)[X] > (_h2)[X] + (_t) && \
01655 (_l1)[Y] > (_h2)[Y] + (_t) && \
01656 (_l1)[Z] > (_h2)[Z] + (_t)) || \
01657 ((_l2)[X] > (_h1)[X] + (_t) && \
01658 (_l2)[Y] > (_h1)[Y] + (_t) && \
01659 (_l2)[Z] > (_h1)[Z] + (_t)))
01660
01661
01662 #define V3RPP_OVERLAP(_l1, _h1, _l2, _h2) \
01663 (! ((_l1)[X] > (_h2)[X] || (_l1)[Y] > (_h2)[Y] || (_l1)[Z] > (_h2)[Z] || \
01664 (_l2)[X] > (_h1)[X] || (_l2)[Y] > (_h1)[Y] || (_l2)[Z] > (_h1)[Z]))
01665
01666
01667
01668
01669
01670 #define V3RPP_OVERLAP_TOL(_l1, _h1, _l2, _h2, _t) \
01671 (! ((_l1)[X] > (_h2)[X] + (_t) || \
01672 (_l1)[Y] > (_h2)[Y] + (_t) || \
01673 (_l1)[Z] > (_h2)[Z] + (_t) || \
01674 (_l2)[X] > (_h1)[X] + (_t) || \
01675 (_l2)[Y] > (_h1)[Y] + (_t) || \
01676 (_l2)[Z] > (_h1)[Z] + (_t)))
01677
01678
01679
01680
01681
01682
01683 #define V3PT_IN_RPP(_pt, _lo, _hi) (\
01684 (_pt)[X] >= (_lo)[X] && (_pt)[X] <= (_hi)[X] && \
01685 (_pt)[Y] >= (_lo)[Y] && (_pt)[Y] <= (_hi)[Y] && \
01686 (_pt)[Z] >= (_lo)[Z] && (_pt)[Z] <= (_hi)[Z])
01687
01688
01689
01690
01691
01692
01693 #define V3PT_IN_RPP_TOL(_pt, _lo, _hi, _t) (\
01694 (_pt)[X] >= (_lo)[X]-(_t) && (_pt)[X] <= (_hi)[X]+(_t) && \
01695 (_pt)[Y] >= (_lo)[Y]-(_t) && (_pt)[Y] <= (_hi)[Y]+(_t) && \
01696 (_pt)[Z] >= (_lo)[Z]-(_t) && (_pt)[Z] <= (_hi)[Z]+(_t))
01697
01698
01699
01700
01701
01702 #define V3PT_OUT_RPP_TOL(_pt, _lo, _hi, _t) (\
01703 (_pt)[X] < (_lo)[X]-(_t) || (_pt)[X] > (_hi)[X]+(_t) || \
01704 (_pt)[Y] < (_lo)[Y]-(_t) || (_pt)[Y] > (_hi)[Y]+(_t) || \
01705 (_pt)[Z] < (_lo)[Z]-(_t) || (_pt)[Z] > (_hi)[Z]+(_t))
01706
01707
01708
01709
01710
01711
01712
01713 #define V3RPP1_IN_RPP2(_lo1, _hi1, _lo2, _hi2) (\
01714 (_lo1)[X] >= (_lo2)[X] && (_hi1)[X] <= (_hi2)[X] && \
01715 (_lo1)[Y] >= (_lo2)[Y] && (_hi1)[Y] <= (_hi2)[Y] && \
01716 (_lo1)[Z] >= (_lo2)[Z] && (_hi1)[Z] <= (_hi2)[Z])
01717
01718
01719 #define V3DIR_FROM_AZEL(_d, _a, _e) { \
01720 register fastf_t _c_e = cos(_e); \
01721 (_d)[X] = cos(_a) * _c_e; \
01722 (_d)[Y] = sin(_a) * _c_e; \
01723 (_d)[Z] = sin(_e); \
01724 }
01725
01726
01727 #define AZEL_FROM_V3DIR(_a, _e, _d) { \
01728 (_a) = ((NEAR_ZERO((_d)[X], SMALL_FASTF)) && (NEAR_ZERO((_d)[Y], SMALL_FASTF))) ? 0.0 : atan2(-((_d)[Y]), -((_d)[X])) * -RAD2DEG; \
01729 (_e) = atan2(-((_d)[Z]), sqrt((_d)[X]*(_d)[X] + (_d)[Y]*(_d)[Y])) * -RAD2DEG; \
01730 }
01731
01732
01733
01734
01735
01736
01737
01738
01739
01740 #define VINITALL(_v) {(_v), (_v), (_v)}
01741
01742
01743
01744
01745
01746
01747
01748 #define V2INITALL(_v) {(_v), (_v), (_v)}
01749
01750
01751
01752
01753
01754
01755
01756 #define HINITALL(_v) {(_v), (_v), (_v), (_v)}
01757
01758
01759
01760
01761
01762
01763
01764 #define VINIT_ZERO {0.0, 0.0, 0.0}
01765
01766
01767
01768
01769
01770
01771
01772 #define V2INIT_ZERO {0.0, 0.0}
01773
01774
01775
01776
01777
01778
01779
01780 #define HINIT_ZERO {0.0, 0.0, 0.0, 0.0}
01781
01782
01783
01784
01785
01786
01787 #define MAT_INIT_IDN {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}
01788
01789
01790
01791
01792
01793
01794 #define MAT_INIT_ZERO {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
01795
01796 __END_DECLS
01797
01798 #endif
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809