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 #include "common.h"
00044
00045 #include <math.h>
00046 #include <string.h>
00047 #include "bio.h"
00048
00049 #include "bu.h"
00050 #include "vmath.h"
00051 #include "bn.h"
00052
00053
00054 const mat_t bn_mat_identity = MAT_INIT_IDN;
00055
00056
00057 void
00058 bn_mat_print_guts(const char *title,
00059 const mat_t m,
00060 char *obuf,
00061 int len)
00062 {
00063 register int i;
00064 register char *cp;
00065
00066 snprintf(obuf, len, "MATRIX %s:\n ", title);
00067 cp = obuf+strlen(obuf);
00068 if (!m) {
00069 bu_strlcat(obuf, "(Identity)", len);
00070 } else {
00071 for (i=0; i<16; i++) {
00072 snprintf(cp, len-(cp-obuf), " %8.3f", m[i]);
00073 cp += strlen(cp);
00074 if (i == 15) {
00075 break;
00076 } else if ((i&3) == 3) {
00077 *cp++ = '\n';
00078 *cp++ = ' ';
00079 *cp++ = ' ';
00080 }
00081 }
00082 *cp++ = '\0';
00083 }
00084 }
00085
00086
00087
00088
00089
00090 void
00091 bn_mat_print(const char *title,
00092 const mat_t m)
00093 {
00094 char obuf[1024];
00095
00096 bn_mat_print_guts(title, m, obuf, 1024);
00097 bu_log("%s\n", obuf);
00098 }
00099
00100
00101
00102
00103
00104 void
00105 bn_mat_print_vls(const char *title,
00106 const mat_t m,
00107 struct bu_vls *vls)
00108 {
00109 char obuf[1024];
00110
00111 bn_mat_print_guts(title, m, obuf, 1024);
00112 bu_vls_printf(vls, "%s\n", obuf);
00113 }
00114
00115
00116
00117
00118
00119
00120
00121
00122 double
00123 bn_atan2(double y, double x)
00124 {
00125 if (x > -1.0e-20 && x < 1.0e-20) {
00126
00127 if (y < -1.0e-20)
00128 return -M_PI_2;
00129 if (y > 1.0e-20)
00130 return M_PI_2;
00131 return 0.0;
00132 }
00133 return atan2(y, x);
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 void
00146 bn_mat_mul(register mat_t o, register const mat_t a, register const mat_t b)
00147 {
00148 o[ 0] = a[ 0]*b[ 0] + a[ 1]*b[ 4] + a[ 2]*b[ 8] + a[ 3]*b[12];
00149 o[ 1] = a[ 0]*b[ 1] + a[ 1]*b[ 5] + a[ 2]*b[ 9] + a[ 3]*b[13];
00150 o[ 2] = a[ 0]*b[ 2] + a[ 1]*b[ 6] + a[ 2]*b[10] + a[ 3]*b[14];
00151 o[ 3] = a[ 0]*b[ 3] + a[ 1]*b[ 7] + a[ 2]*b[11] + a[ 3]*b[15];
00152
00153 o[ 4] = a[ 4]*b[ 0] + a[ 5]*b[ 4] + a[ 6]*b[ 8] + a[ 7]*b[12];
00154 o[ 5] = a[ 4]*b[ 1] + a[ 5]*b[ 5] + a[ 6]*b[ 9] + a[ 7]*b[13];
00155 o[ 6] = a[ 4]*b[ 2] + a[ 5]*b[ 6] + a[ 6]*b[10] + a[ 7]*b[14];
00156 o[ 7] = a[ 4]*b[ 3] + a[ 5]*b[ 7] + a[ 6]*b[11] + a[ 7]*b[15];
00157
00158 o[ 8] = a[ 8]*b[ 0] + a[ 9]*b[ 4] + a[10]*b[ 8] + a[11]*b[12];
00159 o[ 9] = a[ 8]*b[ 1] + a[ 9]*b[ 5] + a[10]*b[ 9] + a[11]*b[13];
00160 o[10] = a[ 8]*b[ 2] + a[ 9]*b[ 6] + a[10]*b[10] + a[11]*b[14];
00161 o[11] = a[ 8]*b[ 3] + a[ 9]*b[ 7] + a[10]*b[11] + a[11]*b[15];
00162
00163 o[12] = a[12]*b[ 0] + a[13]*b[ 4] + a[14]*b[ 8] + a[15]*b[12];
00164 o[13] = a[12]*b[ 1] + a[13]*b[ 5] + a[14]*b[ 9] + a[15]*b[13];
00165 o[14] = a[12]*b[ 2] + a[13]*b[ 6] + a[14]*b[10] + a[15]*b[14];
00166 o[15] = a[12]*b[ 3] + a[13]*b[ 7] + a[14]*b[11] + a[15]*b[15];
00167 }
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178 void
00179 bn_mat_mul2(register const mat_t i, register mat_t o)
00180 {
00181 mat_t temp;
00182
00183 bn_mat_mul(temp, i, o);
00184 MAT_COPY(o, temp);
00185 }
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 void
00197 bn_mat_mul3(mat_t o, const mat_t a, const mat_t b, const mat_t c)
00198 {
00199 mat_t t;
00200
00201 bn_mat_mul(t, b, c);
00202 bn_mat_mul(o, a, t);
00203 }
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213 void
00214 bn_mat_mul4(mat_t ao, const mat_t a, const mat_t b, const mat_t c, const mat_t d)
00215 {
00216 mat_t t, u;
00217
00218 bn_mat_mul(u, c, d);
00219 bn_mat_mul(t, a, b);
00220 bn_mat_mul(ao, t, u);
00221 }
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231 void
00232 bn_matXvec(register vect_t ov, register const mat_t im, register const vect_t iv)
00233 {
00234 register int eo = 0;
00235 register int em = 0;
00236 register int ei;
00237
00238
00239 for (; eo<4; eo++) {
00240
00241 ov[eo] = 0;
00242
00243 for (ei=0; ei<4; ei++)
00244 ov[eo] += im[em++] * iv[ei];
00245 }
00246 }
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257 void
00258 bn_mat_inv(register mat_t output, const mat_t input)
00259 {
00260 if (bn_mat_inverse(output, input) == 0) {
00261
00262 if (bu_debug & BU_DEBUG_MATH) {
00263 bu_log("bn_mat_inv: error!");
00264 bn_mat_print("singular matrix", input);
00265 }
00266 bu_bomb("bn_mat_inv: singular matrix\n");
00267
00268 }
00269 }
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285 int
00286 bn_mat_inverse(register mat_t output, const mat_t input)
00287 {
00288 register int i, j;
00289 int k;
00290 int z[4];
00291 fastf_t b[4];
00292 fastf_t c[4];
00293
00294 MAT_COPY(output, input);
00295
00296
00297 for (j = 0; j < 4; j++)
00298 z[j] = j;
00299
00300
00301 for (i = 0; i < 4; i++) {
00302 register fastf_t y;
00303
00304 k = i;
00305 y = output[i*4+i];
00306 for (j = i+1; j < 4; j++) {
00307 register fastf_t w;
00308
00309 w = output[i*4+j];
00310 if (fabs(w) > fabs(y)) {
00311 k = j;
00312 y = w;
00313 }
00314 }
00315
00316 if (ZERO(y)) {
00317
00318 return 0;
00319 }
00320 y = 1.0 / y;
00321
00322 for (j = 0; j < 4; j++) {
00323 register fastf_t temp;
00324
00325 c[j] = output[j*4+k];
00326 output[j*4+k] = output[j*4+i];
00327 output[j*4+i] = - c[j] * y;
00328 temp = output[i*4+j] * y;
00329 b[j] = temp;
00330 output[i*4+j] = temp;
00331 }
00332
00333 output[i*4+i] = y;
00334 j = z[i];
00335 z[i] = z[k];
00336 z[k] = j;
00337 for (k = 0; k < 4; k++) {
00338 if (k == i)
00339 continue;
00340 for (j = 0; j < 4; j++) {
00341 if (j == i)
00342 continue;
00343 output[k*4+j] = output[k*4+j] - b[j] * c[k];
00344 }
00345 }
00346 }
00347
00348
00349 for (i = 0; i < 4; i++) {
00350 while ((k = z[i]) != i) {
00351 int p;
00352
00353 for (j = 0; j < 4; j++) {
00354 register fastf_t w;
00355
00356 w = output[i*4+j];
00357 output[i*4+j] = output[k*4+j];
00358 output[k*4+j] = w;
00359 }
00360 p = z[i];
00361 z[i] = z[k];
00362 z[k] = p;
00363 }
00364 }
00365
00366 return 1;
00367 }
00368
00369
00370
00371
00372
00373
00374
00375
00376 void
00377 bn_vtoh_move(register vect_t h, register const vect_t v)
00378 {
00379 VMOVE(h, v);
00380 h[W] = 1.0;
00381 }
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393 void
00394 bn_htov_move(register vect_t v, register const vect_t h)
00395 {
00396 register fastf_t inv;
00397
00398 if (ZERO(h[3] - 1.0)) {
00399 VMOVE(v, h);
00400 } else {
00401 if (ZERO(h[W])) {
00402 bu_log("bn_htov_move: divide by %f!\n", h[W]);
00403 return;
00404 }
00405 inv = 1.0 / h[W];
00406 VSCALE(v, h, inv);
00407 }
00408 }
00409
00410
00411
00412
00413
00414 void
00415 bn_mat_trn(mat_t om, register const mat_t im)
00416 {
00417 MAT_TRANSPOSE(om, im);
00418 }
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430 void
00431 bn_mat_ae(register fastf_t *m, double azimuth, double elev)
00432 {
00433 double sin_az, sin_el;
00434 double cos_az, cos_el;
00435
00436 azimuth *= bn_degtorad;
00437 elev *= bn_degtorad;
00438
00439 sin_az = sin(azimuth);
00440 cos_az = cos(azimuth);
00441 sin_el = sin(elev);
00442 cos_el = cos(elev);
00443
00444 m[0] = cos_el * cos_az;
00445 m[1] = -sin_az;
00446 m[2] = -sin_el * cos_az;
00447 m[3] = 0;
00448
00449 m[4] = cos_el * sin_az;
00450 m[5] = cos_az;
00451 m[6] = -sin_el * sin_az;
00452 m[7] = 0;
00453
00454 m[8] = sin_el;
00455 m[9] = 0;
00456 m[10] = cos_el;
00457 m[11] = 0;
00458
00459 m[12] = m[13] = m[14] = 0;
00460 m[15] = 1.0;
00461 }
00462
00463
00464
00465
00466
00467
00468
00469
00470 void
00471 bn_ae_vec(fastf_t *azp, fastf_t *elp, const vect_t v)
00472 {
00473 register fastf_t az;
00474
00475 if ((az = bn_atan2(v[Y], v[X]) * bn_radtodeg) < 0) {
00476 *azp = 360 + az;
00477 } else if (az >= 360) {
00478 *azp = az - 360;
00479 } else {
00480 *azp = az;
00481 }
00482 *elp = bn_atan2(v[Z], hypot(v[X], v[Y])) * bn_radtodeg;
00483 }
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495 void
00496 bn_aet_vec(fastf_t *az, fastf_t *el, fastf_t *twist, fastf_t *vec_ae, fastf_t *vec_twist, fastf_t accuracy)
00497 {
00498 vect_t zero_twist, ninety_twist;
00499 vect_t z_dir;
00500
00501
00502 bn_ae_vec(az, el, vec_ae);
00503
00504
00505
00506 if (NEAR_EQUAL(*az, 360.0, accuracy))
00507 *az = 0.0;
00508
00509
00510 if (NEAR_EQUAL(*el, 90.0, accuracy) || NEAR_ZERO(*el + 90.0, accuracy)) {
00511 *twist = 0.0;
00512 *az = bn_atan2(-vec_twist[X], vec_twist[Y]) * bn_radtodeg;
00513 } else {
00514
00515 VSET(z_dir, 0, 0, 1);
00516 VCROSS(zero_twist, z_dir, vec_ae);
00517 VUNITIZE(zero_twist);
00518 VCROSS(ninety_twist, vec_ae, zero_twist);
00519 VUNITIZE(ninety_twist);
00520
00521 *twist = bn_atan2(VDOT(vec_twist, ninety_twist), VDOT(vec_twist, zero_twist)) * bn_radtodeg;
00522
00523
00524 if (NEAR_EQUAL(*twist, -180.0, accuracy))
00525 *twist = 180.0;
00526 }
00527 }
00528
00529
00530
00531
00532
00533
00534
00535 void
00536 bn_vec_ae(vect_t vect, fastf_t az, fastf_t el)
00537 {
00538 fastf_t vx, vy, vz;
00539 vz = sin(el);
00540 vy = fabs(vz) * sin(az);
00541 vx = fabs(vz) * cos(az);
00542 VSET(vect, vx, vy , vz);
00543 VUNITIZE(vect);
00544 }
00545
00546
00547
00548
00549
00550
00551
00552 void
00553 bn_vec_aed(vect_t vect, fastf_t az, fastf_t el, fastf_t distance)
00554 {
00555 fastf_t vx, vy, vz, rtemp;
00556 vz = distance * sin(el);
00557 rtemp = sqrt((distance * distance) - (vz * vz));
00558 vy = rtemp * sin(az);
00559 vx = rtemp * cos(az);
00560 VSET(vect, vx, vy , vz);
00561 }
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576 void
00577 bn_mat_angles(register fastf_t *mat, double alpha_in, double beta_in, double ggamma_in)
00578 {
00579 double alpha, beta, ggamma;
00580 double calpha, cbeta, cgamma;
00581 double salpha, sbeta, sgamma;
00582
00583 if (NEAR_ZERO(alpha_in, 0.0) && NEAR_ZERO(beta_in, 0.0) && NEAR_ZERO(ggamma_in, 0.0)) {
00584 MAT_IDN(mat);
00585 return;
00586 }
00587
00588 alpha = alpha_in * bn_degtorad;
00589 beta = beta_in * bn_degtorad;
00590 ggamma = ggamma_in * bn_degtorad;
00591
00592 calpha = cos(alpha);
00593 cbeta = cos(beta);
00594 cgamma = cos(ggamma);
00595
00596
00597
00598
00599
00600 if (ZERO(alpha_in - 180.0))
00601 salpha = 0.0;
00602 else
00603 salpha = sin(alpha);
00604
00605 if (ZERO(beta_in - 180.0))
00606 sbeta = 0.0;
00607 else
00608 sbeta = sin(beta);
00609
00610 if (ZERO(ggamma_in - 180.0))
00611 sgamma = 0.0;
00612 else
00613 sgamma = sin(ggamma);
00614
00615 mat[0] = cbeta * cgamma;
00616 mat[1] = -cbeta * sgamma;
00617 mat[2] = sbeta;
00618 mat[3] = 0.0;
00619
00620 mat[4] = salpha * sbeta * cgamma + calpha * sgamma;
00621 mat[5] = -salpha * sbeta * sgamma + calpha * cgamma;
00622 mat[6] = -salpha * cbeta;
00623 mat[7] = 0.0;
00624
00625 mat[8] = salpha * sgamma - calpha * sbeta * cgamma;
00626 mat[9] = salpha * cgamma + calpha * sbeta * sgamma;
00627 mat[10] = calpha * cbeta;
00628 mat[11] = 0.0;
00629 mat[12] = mat[13] = mat[14] = 0.0;
00630 mat[15] = 1.0;
00631 }
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646 void
00647 bn_mat_angles_rad(register mat_t mat,
00648 double alpha,
00649 double beta,
00650 double ggamma)
00651 {
00652 double calpha, cbeta, cgamma;
00653 double salpha, sbeta, sgamma;
00654
00655 if (ZERO(alpha) && ZERO(beta) && ZERO(ggamma)) {
00656 MAT_IDN(mat);
00657 return;
00658 }
00659
00660 calpha = cos(alpha);
00661 cbeta = cos(beta);
00662 cgamma = cos(ggamma);
00663
00664 salpha = sin(alpha);
00665 sbeta = sin(beta);
00666 sgamma = sin(ggamma);
00667
00668 mat[0] = cbeta * cgamma;
00669 mat[1] = -cbeta * sgamma;
00670 mat[2] = sbeta;
00671 mat[3] = 0.0;
00672
00673 mat[4] = salpha * sbeta * cgamma + calpha * sgamma;
00674 mat[5] = -salpha * sbeta * sgamma + calpha * cgamma;
00675 mat[6] = -salpha * cbeta;
00676 mat[7] = 0.0;
00677
00678 mat[8] = salpha * sgamma - calpha * sbeta * cgamma;
00679 mat[9] = salpha * cgamma + calpha * sbeta * sgamma;
00680 mat[10] = calpha * cbeta;
00681 mat[11] = 0.0;
00682 mat[12] = mat[13] = mat[14] = 0.0;
00683 mat[15] = 1.0;
00684 }
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697 void
00698 bn_eigen2x2(fastf_t *val1, fastf_t *val2, fastf_t *vec1, fastf_t *vec2, fastf_t a, fastf_t b, fastf_t c)
00699 {
00700 fastf_t d, root;
00701 fastf_t v1, v2;
00702
00703 d = 0.5 * (c - a);
00704
00705
00706 if (NEAR_ZERO(b, 1.0e-10)) {
00707
00708 if (fabs(c) < fabs(a)) {
00709 *val1 = c;
00710 VSET(vec1, 0.0, 1.0, 0.0);
00711 *val2 = a;
00712 VSET(vec2, -1.0, 0.0, 0.0);
00713 } else {
00714 *val1 = a;
00715 VSET(vec1, 1.0, 0.0, 0.0);
00716 *val2 = c;
00717 VSET(vec2, 0.0, 1.0, 0.0);
00718 }
00719 return;
00720 }
00721
00722 root = sqrt(d*d + b*b);
00723 v1 = 0.5 * (c + a) - root;
00724 v2 = 0.5 * (c + a) + root;
00725
00726
00727 if (fabs(v1) < fabs(v2)) {
00728 *val1 = v1;
00729 *val2 = v2;
00730 VSET(vec1, b, d - root, 0.0);
00731 } else {
00732 *val1 = v2;
00733 *val2 = v1;
00734 VSET(vec1, root - d, b, 0.0);
00735 }
00736 VUNITIZE(vec1);
00737 VSET(vec2, -vec1[Y], vec1[X], 0.0);
00738 }
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750 void
00751 bn_vec_perp(vect_t new, const vect_t old)
00752 {
00753 register int i;
00754 vect_t another;
00755
00756 i = X;
00757 if (fabs(old[Y])<fabs(old[i])) i=Y;
00758 if (fabs(old[Z])<fabs(old[i])) i=Z;
00759 VSETALL(another, 0);
00760 another[i] = 1.0;
00761 if (ZERO(old[X]) && ZERO(old[Y]) && ZERO(old[Z])) {
00762 VMOVE(new, another);
00763 } else {
00764 VCROSS(new, another, old);
00765 }
00766 }
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781 void
00782 bn_mat_fromto(mat_t m, const vect_t from, const vect_t to)
00783 {
00784 vect_t test_to;
00785 vect_t unit_from, unit_to;
00786 fastf_t dot;
00787
00788
00789
00790
00791
00792
00793 mat_t Q, Qt;
00794 mat_t R;
00795 mat_t A;
00796 mat_t temp;
00797 vect_t N, M;
00798 vect_t w_prime;
00799
00800 VMOVE(unit_from, from);
00801 VUNITIZE(unit_from);
00802 VMOVE(unit_to, to);
00803 VUNITIZE(unit_to);
00804
00805
00806
00807
00808
00809 dot = VDOT(unit_from, unit_to);
00810 if (dot > 1.0 - 0.00001) {
00811
00812 MAT_IDN(m);
00813 return;
00814 }
00815 if (dot < -1.0 + 0.00001) {
00816
00817 bn_vec_perp(N, unit_from);
00818 } else {
00819 VCROSS(N, unit_from, unit_to);
00820 VUNITIZE(N);
00821 }
00822 VCROSS(M, N, unit_from);
00823 VUNITIZE(M);
00824
00825
00826 MAT_IDN(Q);
00827 VMOVE(&Q[0], unit_from);
00828 VMOVE(&Q[4], M);
00829 VMOVE(&Q[8], N);
00830 bn_mat_trn(Qt, Q);
00831
00832
00833 MAT4X3VEC(w_prime, Q, unit_to);
00834
00835 MAT_IDN(R);
00836 VMOVE(&R[0], w_prime);
00837 VSET(&R[4], -w_prime[Y], w_prime[X], w_prime[Z]);
00838 VSET(&R[8], 0, 0, 1);
00839
00840 bn_mat_mul(temp, R, Q);
00841 bn_mat_mul(A, Qt, temp);
00842 bn_mat_trn(m, A);
00843
00844
00845 MAT4X3VEC(test_to, m, unit_from);
00846 dot = VDOT(unit_to, test_to);
00847 if (UNLIKELY(dot < 0.98 || dot > 1.02)) {
00848 bu_log("bn_mat_fromto() ERROR! from (%g, %g, %g) to (%g, %g, %g) went to (%g, %g, %g), dot=%g?\n",
00849 V3ARGS(from),
00850 V3ARGS(to),
00851 V3ARGS(test_to), dot);
00852 }
00853 }
00854
00855
00856
00857
00858
00859
00860
00861
00862 void
00863 bn_mat_xrot(fastf_t *m, double sinx, double cosx)
00864 {
00865 m[0] = 1.0;
00866 m[1] = 0.0;
00867 m[2] = 0.0;
00868 m[3] = 0.0;
00869
00870 m[4] = 0.0;
00871 m[5] = cosx;
00872 m[6] = -sinx;
00873 m[7] = 0.0;
00874
00875 m[8] = 0.0;
00876 m[9] = sinx;
00877 m[10] = cosx;
00878 m[11] = 0.0;
00879
00880 m[12] = m[13] = m[14] = 0.0;
00881 m[15] = 1.0;
00882 }
00883
00884
00885
00886
00887
00888
00889
00890
00891 void
00892 bn_mat_yrot(fastf_t *m, double siny, double cosy)
00893 {
00894 m[0] = cosy;
00895 m[1] = 0.0;
00896 m[2] = -siny;
00897 m[3] = 0.0;
00898
00899 m[4] = 0.0;
00900 m[5] = 1.0;
00901 m[6] = 0.0;
00902 m[7] = 0.0;
00903
00904 m[8] = siny;
00905 m[9] = 0.0;
00906 m[10] = cosy;
00907
00908 m[11] = 0.0;
00909 m[12] = m[13] = m[14] = 0.0;
00910 m[15] = 1.0;
00911 }
00912
00913
00914
00915
00916
00917
00918
00919
00920 void
00921 bn_mat_zrot(fastf_t *m, double sinz, double cosz)
00922 {
00923 m[0] = cosz;
00924 m[1] = -sinz;
00925 m[2] = 0.0;
00926 m[3] = 0.0;
00927
00928 m[4] = sinz;
00929 m[5] = cosz;
00930 m[6] = 0.0;
00931 m[7] = 0.0;
00932
00933 m[8] = 0.0;
00934 m[9] = 0.0;
00935 m[10] = 1.0;
00936 m[11] = 0.0;
00937
00938 m[12] = m[13] = m[14] = 0.0;
00939 m[15] = 1.0;
00940 }
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965 void
00966 bn_mat_lookat(mat_t rot, const vect_t dir, int yflip)
00967 {
00968 mat_t first;
00969 mat_t second;
00970 mat_t prod12;
00971 mat_t third;
00972 vect_t x;
00973 vect_t z;
00974 vect_t t1;
00975 fastf_t hypot_xy;
00976 vect_t xproj;
00977 vect_t zproj;
00978
00979
00980 hypot_xy = hypot(dir[X], dir[Y]);
00981 bn_mat_zrot(first, -dir[Y] / hypot_xy, dir[X] / hypot_xy);
00982
00983
00984 bn_mat_yrot(second, -hypot_xy, -dir[Z]);
00985 bn_mat_mul(prod12, second, first);
00986
00987
00988 VSET(x, 1, 0, 0);
00989 MAT4X3VEC(xproj, prod12, x);
00990 hypot_xy = hypot(xproj[X], xproj[Y]);
00991 if (hypot_xy < 1.0e-10) {
00992 bu_log("Warning: bn_mat_lookat: unable to twist correct, hypot=%g\n", hypot_xy);
00993 VPRINT("xproj", xproj);
00994 MAT_COPY(rot, prod12);
00995 return;
00996 }
00997 bn_mat_zrot(third, -xproj[Y] / hypot_xy, xproj[X] / hypot_xy);
00998 bn_mat_mul(rot, third, prod12);
00999
01000 if (yflip) {
01001 VSET(z, 0, 0, 1);
01002 MAT4X3VEC(zproj, rot, z);
01003
01004 if (zproj[Y] < 0.0) {
01005 MAT_COPY(prod12, rot);
01006 MAT_IDN(third);
01007 third[5] = -1;
01008 bn_mat_mul(rot, third, prod12);
01009 }
01010 }
01011
01012
01013 MAT4X3VEC(t1, rot, dir);
01014 if (t1[Z] > -0.98) {
01015 bu_log("Error: bn_mat_lookat final= (%g, %g, %g)\n", V3ARGS(t1));
01016 }
01017 }
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029 void
01030 bn_vec_ortho(register vect_t out, register const vect_t in)
01031 {
01032 register int j, k;
01033 register fastf_t f;
01034 register int i;
01035
01036 if (UNLIKELY(NEAR_ZERO(MAGSQ(in), SQRT_SMALL_FASTF))) {
01037 bu_log("bn_vec_ortho(): zero magnitude input vector %g %g %g\n", V3ARGS(in));
01038 VSETALL(out, 0);
01039 return;
01040 }
01041
01042
01043 f = fabs(in[X]);
01044 i = X;
01045 j = Y;
01046 k = Z;
01047 if (fabs(in[Y]) < f) {
01048 f = fabs(in[Y]);
01049 i = Y;
01050 j = Z;
01051 k = X;
01052 }
01053 if (fabs(in[Z]) < f) {
01054 i = Z;
01055 j = X;
01056 k = Y;
01057 }
01058 f = hypot(in[j], in[k]);
01059 if (UNLIKELY(ZERO(f))) {
01060 bu_log("bn_vec_ortho(): zero hypot on %g %g %g\n", V3ARGS(in));
01061 VSETALL(out, 0);
01062 return;
01063 }
01064 f = 1.0/f;
01065 out[i] = 0.0;
01066 out[j] = -in[k]*f;
01067 out[k] = in[j]*f;
01068
01069 return;
01070 }
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083 int
01084 bn_mat_scale_about_pt(mat_t mat, const point_t pt, const double scale)
01085 {
01086 mat_t xlate;
01087 mat_t s;
01088 mat_t tmp;
01089
01090 MAT_IDN(xlate);
01091 MAT_DELTAS_VEC_NEG(xlate, pt);
01092
01093 MAT_IDN(s);
01094 if (ZERO(scale)) {
01095 MAT_ZERO(mat);
01096 return -1;
01097 }
01098 s[15] = 1/scale;
01099
01100 bn_mat_mul(tmp, s, xlate);
01101
01102 MAT_DELTAS_VEC(xlate, pt);
01103 bn_mat_mul(mat, xlate, tmp);
01104 return 0;
01105 }
01106
01107
01108
01109
01110
01111
01112
01113
01114 void
01115 bn_mat_xform_about_pt(mat_t mat, const mat_t xform, const point_t pt)
01116 {
01117 mat_t xlate;
01118 mat_t tmp;
01119
01120 MAT_IDN(xlate);
01121 MAT_DELTAS_VEC_NEG(xlate, pt);
01122
01123 bn_mat_mul(tmp, xform, xlate);
01124
01125 MAT_DELTAS_VEC(xlate, pt);
01126 bn_mat_mul(mat, xlate, tmp);
01127 }
01128
01129
01130
01131
01132
01133
01134
01135
01136 int
01137 bn_mat_is_equal(const mat_t a, const mat_t b, const struct bn_tol *tol)
01138 {
01139 register int i;
01140 register double f;
01141 register double tdist, tperp;
01142
01143 BN_CK_TOL(tol);
01144
01145 tdist = tol->dist;
01146 tperp = tol->perp;
01147
01148
01149
01150
01151
01152
01153 for (i=3; i<12; i+=4) {
01154 f = a[i] - b[i];
01155 if (!NEAR_ZERO(f, tdist)) return 0;
01156 }
01157
01158
01159
01160
01161 for (i = 0; i < 16; i+=4) {
01162 f = a[i] - b[i];
01163 if (!NEAR_ZERO(f, tperp)) return 0;
01164 f = a[i+1] - b[i+1];
01165 if (!NEAR_ZERO(f, tperp)) return 0;
01166 f = a[i+2] - b[i+2];
01167 if (!NEAR_ZERO(f, tperp)) return 0;
01168 }
01169
01170
01171
01172
01173
01174 f = a[15] - b[15];
01175 if (!NEAR_ZERO(f, tperp)) return 0;
01176
01177 return 1;
01178 }
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193
01194 int
01195 bn_mat_is_identity(const mat_t m)
01196 {
01197 return ! memcmp(m, bn_mat_identity, sizeof(mat_t));
01198 }
01199
01200
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210 void
01211 bn_mat_arb_rot(mat_t m, const point_t pt, const vect_t dir, const fastf_t ang)
01212 {
01213 mat_t tran1, tran2, rot;
01214 double cos_ang, sin_ang, one_m_cosang;
01215 double n1_sq, n2_sq, n3_sq;
01216 double n1_n2, n1_n3, n2_n3;
01217
01218 if (ZERO(ang)) {
01219 MAT_IDN(m);
01220 return;
01221 }
01222
01223 MAT_IDN(tran1);
01224 MAT_IDN(tran2);
01225
01226
01227 tran1[MDX] = (-pt[X]);
01228 tran1[MDY] = (-pt[Y]);
01229 tran1[MDZ] = (-pt[Z]);
01230
01231
01232 tran2[MDX] = pt[X];
01233 tran2[MDY] = pt[Y];
01234 tran2[MDZ] = pt[Z];
01235
01236
01237 cos_ang = cos(ang);
01238 sin_ang = sin(ang);
01239 one_m_cosang = 1.0 - cos_ang;
01240 n1_sq = dir[X]*dir[X];
01241 n2_sq = dir[Y]*dir[Y];
01242 n3_sq = dir[Z]*dir[Z];
01243 n1_n2 = dir[X]*dir[Y];
01244 n1_n3 = dir[X]*dir[Z];
01245 n2_n3 = dir[Y]*dir[Z];
01246
01247 MAT_IDN(rot);
01248 rot[0] = n1_sq + (1.0 - n1_sq)*cos_ang;
01249 rot[1] = n1_n2 * one_m_cosang - dir[Z]*sin_ang;
01250 rot[2] = n1_n3 * one_m_cosang + dir[Y]*sin_ang;
01251
01252 rot[4] = n1_n2 * one_m_cosang + dir[Z]*sin_ang;
01253 rot[5] = n2_sq + (1.0 - n2_sq)*cos_ang;
01254 rot[6] = n2_n3 * one_m_cosang - dir[X]*sin_ang;
01255
01256 rot[8] = n1_n3 * one_m_cosang - dir[Y]*sin_ang;
01257 rot[9] = n2_n3 * one_m_cosang + dir[X]*sin_ang;
01258 rot[10] = n3_sq + (1.0 - n3_sq) * cos_ang;
01259
01260 bn_mat_mul(m, rot, tran1);
01261 bn_mat_mul2(tran2, m);
01262 }
01263
01264
01265
01266
01267
01268
01269
01270
01271 matp_t
01272 bn_mat_dup(const mat_t in)
01273 {
01274 matp_t out;
01275
01276 out = (matp_t) bu_malloc(sizeof(mat_t), "bn_mat_dup");
01277 memcpy((char *)out, (const char *)in, sizeof(mat_t));
01278 return out;
01279 }
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290
01291
01292
01293 int
01294 bn_mat_ck(const char *title, const mat_t m)
01295 {
01296 vect_t A, B, C;
01297 fastf_t fx, fy, fz;
01298
01299 if (!m) return 0;
01300
01301
01302
01303
01304
01305 VMOVE(A, &m[0]);
01306 VMOVE(B, &m[4]);
01307 VMOVE(C, &m[8]);
01308
01309 fx = VDOT(A, B);
01310 fy = VDOT(B, C);
01311 fz = VDOT(A, C);
01312
01313
01314
01315
01316
01317
01318 if (!NEAR_ZERO(fx, 0.00001)
01319 || !NEAR_ZERO(fy, 0.00001)
01320 || !NEAR_ZERO(fz, 0.00001)
01321 || NEAR_ZERO(m[15], VDIVIDE_TOL))
01322 {
01323 if (bu_debug & BU_DEBUG_MATH) {
01324 bu_log("bn_mat_ck(%s): bad matrix, does not preserve axis perpendicularity.\n X.Y=%g, Y.Z=%g, X.Z=%g, s=%g\n", title, fx, fy, fz, m[15]);
01325 bn_mat_print("bn_mat_ck() bad matrix", m);
01326 }
01327
01328 if (bu_debug & (BU_DEBUG_MATH | BU_DEBUG_COREDUMP)) {
01329 bu_debug |= BU_DEBUG_COREDUMP;
01330 bu_bomb("bn_mat_ck() bad matrix\n");
01331 }
01332 return -1;
01333 }
01334 return 0;
01335 }
01336
01337
01338
01339
01340
01341
01342
01343
01344 fastf_t
01345 bn_mat_det3(const mat_t m)
01346 {
01347 register fastf_t sum;
01348
01349 sum = m[0] * (m[5]*m[10] - m[6]*m[9])
01350 -m[1] * (m[4]*m[10] - m[6]*m[8])
01351 +m[2] * (m[4]*m[9] - m[5]*m[8]);
01352
01353 return sum;
01354 }
01355
01356
01357
01358
01359
01360
01361
01362 fastf_t
01363 bn_mat_determinant(const mat_t m)
01364 {
01365 fastf_t det[4];
01366 fastf_t sum;
01367
01368 det[0] = m[5] * (m[10]*m[15] - m[11]*m[14])
01369 -m[6] * (m[ 9]*m[15] - m[11]*m[13])
01370 +m[7] * (m[ 9]*m[14] - m[10]*m[13]);
01371
01372 det[1] = m[4] * (m[10]*m[15] - m[11]*m[14])
01373 -m[6] * (m[ 8]*m[15] - m[11]*m[12])
01374 +m[7] * (m[ 8]*m[14] - m[10]*m[12]);
01375
01376 det[2] = m[4] * (m[ 9]*m[15] - m[11]*m[13])
01377 -m[5] * (m[ 8]*m[15] - m[11]*m[12])
01378 +m[7] * (m[ 8]*m[13] - m[ 9]*m[12]);
01379
01380 det[3] = m[4] * (m[ 9]*m[14] - m[10]*m[13])
01381 -m[5] * (m[ 8]*m[14] - m[10]*m[12])
01382 +m[6] * (m[ 8]*m[13] - m[ 9]*m[12]);
01383
01384 sum = m[0]*det[0] - m[1]*det[1] + m[2]*det[2] - m[3]*det[3];
01385
01386 return sum;
01387
01388 }
01389
01390
01391
01392
01393
01394
01395
01396 int
01397 bn_mat_is_non_unif (const mat_t m)
01398 {
01399 double mag[3];
01400
01401 mag[0] = MAGSQ(m);
01402 mag[1] = MAGSQ(&m[4]);
01403 mag[2] = MAGSQ(&m[8]);
01404
01405 if (fabs(1.0 - (mag[1]/mag[0])) > .0005 ||
01406 fabs(1.0 - (mag[2]/mag[0])) > .0005) {
01407
01408 return 1;
01409 }
01410
01411 if (!ZERO(m[12]) || !ZERO(m[13]) || !ZERO(m[14]))
01412 return 2;
01413
01414 return 0;
01415 }
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425 void
01426 bn_wrt_point_direc(mat_t out, const mat_t change, const mat_t in, const point_t point, const vect_t direc)
01427 {
01428 static mat_t t1;
01429 static mat_t pt_to_origin, origin_to_pt;
01430 static mat_t d_to_zaxis, zaxis_to_d;
01431 static vect_t zaxis;
01432
01433
01434 MAT_IDN(pt_to_origin);
01435 MAT_DELTAS_VEC_NEG(pt_to_origin, point);
01436
01437
01438 MAT_IDN(origin_to_pt);
01439 MAT_DELTAS_VEC_NEG(origin_to_pt, point);
01440
01441
01442 VSET(zaxis, 0.0, 0.0, 1.0);
01443 bn_mat_fromto(d_to_zaxis, direc, zaxis);
01444
01445
01446 bn_mat_inv(zaxis_to_d, d_to_zaxis);
01447
01448
01449
01450
01451 bn_mat_mul4(t1, change, d_to_zaxis, pt_to_origin, in);
01452
01453
01454
01455
01456
01457 bn_mat_mul3(out, origin_to_pt, zaxis_to_d, t1);
01458 }
01459
01460
01461
01462
01463
01464
01465
01466
01467
01468
01469
01470