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 #include "common.h"
00030
00031 #include <stdio.h>
00032 #include <math.h>
00033 #include <string.h>
00034
00035 #include "bu.h"
00036 #include "vmath.h"
00037 #include "bn.h"
00038
00039 #define UNIT_SQ_TOL 1.0e-13
00040
00041
00042
00043
00044
00045
00046 double
00047 bn_dist_pt3_pt3(const fastf_t *a, const fastf_t *b)
00048 {
00049 vect_t diff;
00050
00051 VSUB2(diff, a, b);
00052 return MAGNITUDE(diff);
00053 }
00054
00055
00056
00057
00058
00059
00060
00061
00062 int
00063 bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
00064 {
00065 register fastf_t tmp = tol->dist_sq;
00066 register fastf_t ab, abx, aby, abz;
00067
00068 abx = a[X]-b[X];
00069 ab = abx * abx;
00070 if (ab > tmp) {
00071 return 0;
00072 }
00073 aby = a[Y]-b[Y];
00074 ab += (aby * aby);
00075 if (ab > tmp) {
00076 return 0;
00077 }
00078 abz = a[Z]-b[Z];
00079 ab += (abz * abz);
00080 if (ab > tmp) {
00081 return 0;
00082 }
00083
00084 return 1;
00085 }
00086
00087
00088
00089
00090
00091
00092
00093
00094 int
00095 bn_pt2_pt2_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
00096 {
00097 vect_t diff;
00098
00099 BN_CK_TOL(tol);
00100 VSUB2_2D(diff, b, a);
00101 if (MAGSQ_2D(diff) < tol->dist_sq) return 1;
00102 return 0;
00103 }
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 int
00118 bn_3pts_collinear(fastf_t *a, fastf_t *b, fastf_t *c, const struct bn_tol *tol)
00119 {
00120 fastf_t mag_ab, mag_bc, mag_ca, max_len, dist_sq;
00121 fastf_t cos_a, cos_b, cos_c;
00122 vect_t ab, bc, ca;
00123 int max_edge_no;
00124
00125 VSUB2(ab, b, a);
00126 VSUB2(bc, c, b);
00127 VSUB2(ca, a, c);
00128 mag_ab = MAGNITUDE(ab);
00129 mag_bc = MAGNITUDE(bc);
00130 mag_ca = MAGNITUDE(ca);
00131
00132
00133 max_len = mag_ab;
00134 max_edge_no = 1;
00135
00136 if (mag_bc > max_len) {
00137 max_len = mag_bc;
00138 max_edge_no = 2;
00139 }
00140
00141 if (mag_ca > max_len) {
00142 max_len = mag_ca;
00143 max_edge_no = 3;
00144 }
00145
00146 switch (max_edge_no) {
00147 default:
00148 case 1:
00149 cos_b = (-VDOT(ab, bc))/(mag_ab * mag_bc);
00150 dist_sq = mag_bc*mag_bc*(1.0 - cos_b*cos_b);
00151 break;
00152 case 2:
00153 cos_c = (-VDOT(bc, ca))/(mag_bc * mag_ca);
00154 dist_sq = mag_ca*mag_ca*(1.0 - cos_c*cos_c);
00155 break;
00156 case 3:
00157 cos_a = (-VDOT(ca, ab))/(mag_ca * mag_ab);
00158 dist_sq = mag_ab*mag_ab*(1.0 - cos_a*cos_a);
00159 break;
00160 }
00161
00162 if (dist_sq <= tol->dist_sq)
00163 return 1;
00164 else
00165 return 0;
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179 int
00180 bn_3pts_distinct(const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol)
00181 {
00182 vect_t B_A;
00183 vect_t C_A;
00184 vect_t C_B;
00185
00186 BN_CK_TOL(tol);
00187 VSUB2(B_A, b, a);
00188 if (MAGSQ(B_A) <= tol->dist_sq) return 0;
00189 VSUB2(C_A, c, a);
00190 if (MAGSQ(C_A) <= tol->dist_sq) return 0;
00191 VSUB2(C_B, c, b);
00192 if (MAGSQ(C_B) <= tol->dist_sq) return 0;
00193 return 1;
00194 }
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207 int
00208 bn_npts_distinct(const int npt, const point_t *pts, const struct bn_tol *tol)
00209 {
00210 int i, j;
00211 point_t r;
00212
00213 BN_CK_TOL(tol);
00214
00215 for (i=0;i<npt;i++)
00216 for (j=i+1;j<npt;j++) {
00217 VSUB2(r, pts[i], pts[j]);
00218 if (MAGSQ(r) <= tol->dist_sq)
00219 return 0;
00220 }
00221 return 1;
00222 }
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269 int
00270 bn_mk_plane_3pts(fastf_t *plane,
00271 const fastf_t *a,
00272 const fastf_t *b,
00273 const fastf_t *c,
00274 const struct bn_tol *tol)
00275 {
00276 vect_t B_A;
00277 vect_t C_A;
00278 vect_t C_B;
00279 register fastf_t mag;
00280
00281 BN_CK_TOL(tol);
00282
00283 VSUB2(B_A, b, a);
00284 if (MAGSQ(B_A) <= tol->dist_sq) return -1;
00285 VSUB2(C_A, c, a);
00286 if (MAGSQ(C_A) <= tol->dist_sq) return -1;
00287 VSUB2(C_B, c, b);
00288 if (MAGSQ(C_B) <= tol->dist_sq) return -1;
00289
00290 VCROSS(plane, B_A, C_A);
00291
00292
00293 if ((mag = MAGNITUDE(plane)) <= SMALL_FASTF)
00294 return -1;
00295 mag = 1/mag;
00296 VSCALE(plane, plane, mag);
00297
00298
00299
00300 plane[3] = VDOT(plane, a);
00301
00302 return 0;
00303 }
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334 int
00335 bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c)
00336 {
00337 vect_t v1;
00338 fastf_t dot;
00339
00340
00341
00342
00343 VCROSS(v1, b, c);
00344
00345
00346
00347
00348
00349
00350
00351
00352 dot = VDOT(a, v1);
00353
00354 if (ZERO(dot)) {
00355 return -1;
00356 } else {
00357 vect_t v2, v3;
00358 fastf_t det, aH, bH, cH;
00359
00360 VCROSS(v2, a, c);
00361 VCROSS(v3, a, b);
00362
00363
00364
00365
00366
00367 aH = MAGNITUDE(a) * a[H];
00368 bH = MAGNITUDE(b) * b[H];
00369 cH = MAGNITUDE(c) * c[H];
00370
00371
00372
00373
00374
00375
00376
00377 det = 1 / dot;
00378
00379 pt[X] = det * (aH * v1[X] - bH * v2[X] + cH * v3[X]);
00380 pt[Y] = det * (aH * v1[Y] - bH * v2[Y] + cH * v3[Y]);
00381 pt[Z] = det * (aH * v1[Z] - bH * v2[Z] + cH * v3[Z]);
00382 }
00383 return 0;
00384 }
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398 int
00399 bn_2line3_colinear(const fastf_t *p1,
00400 const fastf_t *d1,
00401 const fastf_t *p2,
00402 const fastf_t *d2,
00403 double range,
00404 const struct bn_tol *tol)
00405 {
00406 fastf_t mag1;
00407 fastf_t mag2;
00408 point_t tail;
00409
00410 BN_CK_TOL(tol);
00411
00412 if (!p1 || !d1 || !p2 || !d2) {
00413 goto fail;
00414 }
00415
00416 if ((mag1 = MAGNITUDE(d1)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag1 zero\n");
00417 if ((mag2 = MAGNITUDE(d2)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag2 zero\n");
00418
00419
00420
00421 if (fabs(VDOT(d1, d2)) < 0.9 * mag1 * mag2) goto fail;
00422
00423
00424 if (bn_distsq_line3_pt3(p1, d1, p2) > tol->dist_sq) goto fail;
00425 if (bn_distsq_line3_pt3(p2, d2, p1) > tol->dist_sq) goto fail;
00426
00427 VJOIN1(tail, p1, range/mag1, d1);
00428 if (bn_distsq_line3_pt3(p2, d2, tail) > tol->dist_sq) goto fail;
00429
00430 VJOIN1(tail, p2, range/mag2, d2);
00431 if (bn_distsq_line3_pt3(p1, d1, tail) > tol->dist_sq) goto fail;
00432
00433 if (bu_debug & BU_DEBUG_MATH) {
00434 bu_log("bn_2line3colinear(range=%g) ret=1\n", range);
00435 }
00436 return 1;
00437 fail:
00438 if (bu_debug & BU_DEBUG_MATH) {
00439 bu_log("bn_2line3colinear(range=%g) ret=0\n", range);
00440 }
00441 return 0;
00442 }
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470 int
00471 bn_dist_pt3_line3(fastf_t *dist, fastf_t *pca, const fastf_t *a, const fastf_t *dir, const fastf_t *p, const struct bn_tol *tol)
00472 {
00473 vect_t AtoP;
00474 vect_t unit_dir;
00475 fastf_t A_P_sq;
00476 fastf_t t;
00477 fastf_t dsq;
00478
00479 if (UNLIKELY(bu_debug & BU_DEBUG_MATH))
00480 bu_log("bn_dist_pt3_line3(a=(%f %f %f), dir=(%f %f %f), p=(%f %f %f)\n" ,
00481 V3ARGS(a), V3ARGS(dir), V3ARGS(p));
00482
00483 BN_CK_TOL(tol);
00484
00485
00486 VSUB2(AtoP, p, a);
00487 A_P_sq = MAGSQ(AtoP);
00488 if (A_P_sq < tol->dist_sq) {
00489
00490 VMOVE(pca, a);
00491 *dist = 0.0;
00492 return 0;
00493 }
00494
00495 VMOVE(unit_dir, dir);
00496 VUNITIZE(unit_dir);
00497
00498
00499
00500
00501 t = VDOT(AtoP, unit_dir);
00502
00503 VJOIN1(pca, a, t, unit_dir);
00504 dsq = A_P_sq - t*t;
00505 if (dsq < tol->dist_sq) {
00506
00507 *dist = 0.0;
00508 return 1;
00509 } else {
00510
00511 *dist = sqrt(dsq);
00512 return 2;
00513 }
00514 }
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536 int
00537 bn_dist_line3_line3(fastf_t *dist, const fastf_t *p1, const fastf_t *d1, const fastf_t *p2, const fastf_t *d2, const struct bn_tol *tol)
00538 {
00539 fastf_t d1_d2;
00540 point_t a1, a2;
00541 vect_t a1_to_a2;
00542 vect_t p2_to_p1;
00543 fastf_t min_dist;
00544 fastf_t tol_dist_sq;
00545 fastf_t tol_dist;
00546
00547 BN_CK_TOL(tol);
00548
00549 if (tol->dist > 0.0)
00550 tol_dist = tol->dist;
00551 else
00552 tol_dist = 0.0005;
00553
00554 if (tol->dist_sq > 0.0)
00555 tol_dist_sq = tol->dist_sq;
00556 else
00557 tol_dist_sq = tol_dist * tol_dist;
00558
00559 if (!NEAR_EQUAL(MAGSQ(d1), 1.0, tol_dist_sq)) {
00560 bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d1));
00561 bu_bomb("bn_dist_line3_line3\n");
00562 }
00563
00564 if (!NEAR_EQUAL(MAGSQ(d2), 1.0, tol_dist_sq)) {
00565 bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d2));
00566 bu_bomb("bn_dist_line3_line3\n");
00567 }
00568
00569 d1_d2 = VDOT(d1, d2);
00570
00571 if (BN_VECT_ARE_PARALLEL(d1_d2, tol)) {
00572 if (bn_dist_line3_pt3(p1, d1, p2) > tol_dist)
00573 return -2;
00574 else
00575 return -1;
00576 }
00577
00578 VSUB2(p2_to_p1, p1, p2);
00579 dist[0] = (d1_d2 * VDOT(p2_to_p1, d2) - VDOT(p2_to_p1, d1))/(1.0 - d1_d2 * d1_d2);
00580 dist[1] = dist[0] * d1_d2 + VDOT(p2_to_p1, d2);
00581
00582 VJOIN1(a1, p1, dist[0], d1);
00583 VJOIN1(a2, p2, dist[1], d2);
00584
00585 VSUB2(a1_to_a2, a2, a1);
00586 min_dist = MAGNITUDE(a1_to_a2);
00587 if (min_dist < tol_dist)
00588 return 0;
00589 else
00590 return 1;
00591 }
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615 int
00616 bn_dist_line3_lseg3(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
00617 {
00618 vect_t a_to_b;
00619 vect_t a_dir;
00620 fastf_t len_ab;
00621 int outside_segment;
00622 int ret;
00623
00624 BN_CK_TOL(tol);
00625
00626 VSUB2(a_to_b, b, a);
00627 len_ab = MAGNITUDE(a_to_b);
00628 VSCALE(a_dir, a_to_b, (1.0/len_ab));
00629
00630 ret = bn_dist_line3_line3(dist, p, d, a, a_dir, tol);
00631
00632 if (ret < 0) {
00633 vect_t to_a, to_b;
00634 fastf_t dist_to_a, dist_to_b;
00635
00636 VSUB2(to_a, a, p);
00637 VSUB2(to_b, b, p);
00638 dist_to_a = VDOT(to_a, d);
00639 dist_to_b = VDOT(to_b, d);
00640
00641 if (dist_to_a <= dist_to_b) {
00642 dist[0] = dist_to_a;
00643 dist[1] = 0.0;
00644 } else {
00645 dist[0] = dist_to_b;
00646 dist[1] = 1.0;
00647 }
00648 return ret;
00649 }
00650
00651 if (dist[1] >= (-tol->dist) && dist[1] <= len_ab + tol->dist) {
00652
00653 outside_segment = 0;
00654 dist[1] = dist[1]/len_ab;
00655 if (dist[1] < 0.0)
00656 dist[1] = 0.0;
00657 if (dist[1] > 1.0)
00658 dist[1] = 1.0;
00659 } else {
00660 outside_segment = 1;
00661 dist[1] = dist[1]/len_ab;
00662 }
00663
00664 return 2*ret + outside_segment;
00665 }
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688 int
00689 bn_isect_line3_plane(fastf_t *dist,
00690 const fastf_t *pt,
00691 const fastf_t *dir,
00692 const fastf_t *plane,
00693 const struct bn_tol *tol)
00694 {
00695 register fastf_t slant_factor;
00696 register fastf_t norm_dist;
00697 register fastf_t dot;
00698 vect_t local_dir;
00699
00700 BN_CK_TOL(tol);
00701
00702 norm_dist = plane[3] - VDOT(plane, pt);
00703 slant_factor = VDOT(plane, dir);
00704 VMOVE(local_dir, dir);
00705 VUNITIZE(local_dir);
00706 dot = VDOT(plane, local_dir);
00707
00708 if (slant_factor < -SMALL_FASTF && dot < -tol->perp) {
00709 *dist = norm_dist/slant_factor;
00710 return 1;
00711 } else if (slant_factor > SMALL_FASTF && dot > tol->perp) {
00712 *dist = norm_dist/slant_factor;
00713 return 2;
00714 }
00715
00716
00717
00718
00719 *dist = 0;
00720 if (norm_dist < -tol->dist)
00721 return -2;
00722 if (norm_dist > tol->dist)
00723 return -1;
00724 return 0;
00725 }
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752 int
00753 bn_isect_2planes(fastf_t *pt,
00754 fastf_t *dir,
00755 const fastf_t *a,
00756 const fastf_t *b,
00757 const fastf_t *rpp_min,
00758 const struct bn_tol *tol)
00759 {
00760 vect_t abs_dir;
00761 plane_t pl;
00762 int i;
00763
00764 VSETALL(pt, 0.0);
00765 VSETALL(dir, 0.0);
00766
00767 if ((i = bn_coplanar(a, b, tol)) != 0) {
00768 if (i > 0) {
00769 return -1;
00770 }
00771 return -2;
00772 }
00773
00774
00775
00776
00777 VCROSS(dir, a, b);
00778
00779
00780
00781
00782
00783
00784
00785 abs_dir[X] = fabs(dir[X]);
00786 abs_dir[Y] = fabs(dir[Y]);
00787 abs_dir[Z] = fabs(dir[Z]);
00788
00789 if (ZERO(abs_dir[X])) {
00790 abs_dir[X] = 0.0;
00791 }
00792 if (ZERO(abs_dir[Y])) {
00793 abs_dir[Y] = 0.0;
00794 }
00795 if (ZERO(abs_dir[Z])) {
00796 abs_dir[Z] = 0.0;
00797 }
00798
00799 if (abs_dir[X] >= abs_dir[Y]) {
00800 if (abs_dir[X] >= abs_dir[Z]) {
00801 VSET(pl, 1, 0, 0);
00802 pl[W] = rpp_min[X];
00803 if (dir[X] < -SMALL_FASTF) {
00804 VREVERSE(dir, dir);
00805 }
00806 } else {
00807 VSET(pl, 0, 0, 1);
00808 pl[W] = rpp_min[Z];
00809 if (dir[Z] < -SMALL_FASTF) {
00810 VREVERSE(dir, dir);
00811 }
00812 }
00813 } else {
00814 if (abs_dir[Y] >= abs_dir[Z]) {
00815 VSET(pl, 0, 1, 0);
00816 pl[W] = rpp_min[Y];
00817 if (dir[Y] < -SMALL_FASTF) {
00818 VREVERSE(dir, dir);
00819 }
00820 } else {
00821 VSET(pl, 0, 0, 1);
00822 pl[W] = rpp_min[Z];
00823 if (dir[Z] < -SMALL_FASTF) {
00824 VREVERSE(dir, dir);
00825 }
00826 }
00827 }
00828
00829
00830 if (bn_mkpoint_3planes(pt, pl, a, b) < 0) {
00831 return -3;
00832 }
00833
00834
00835 return 0;
00836 }
00837
00838
00839
00840
00841
00842
00843
00844
00845
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881 int
00882 bn_isect_line2_line2(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *c, const struct bn_tol *tol)
00883
00884
00885
00886 {
00887 fastf_t hx, hy;
00888 register fastf_t det;
00889 register fastf_t det1;
00890 vect_t unit_d;
00891 vect_t unit_c;
00892 vect_t unit_h;
00893 int parallel;
00894 int parallel1;
00895
00896 BN_CK_TOL(tol);
00897 if (bu_debug & BU_DEBUG_MATH) {
00898 bu_log("bn_isect_line2_line2() p=(%g, %g), d=(%g, %g)\n\t\t\ta=(%g, %g), c=(%g, %g)\n",
00899 V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
00900 }
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933 det = c[X] * d[Y] - d[X] * c[Y];
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958 hx = a[X] - p[X];
00959 hy = a[Y] - p[Y];
00960 det1 = (c[X] * hy - hx * c[Y]);
00961
00962 unit_d[0] = d[0];
00963 unit_d[1] = d[1];
00964 unit_d[2] = 0.0;
00965 VUNITIZE(unit_d);
00966 unit_c[0] = c[0];
00967 unit_c[1] = c[1];
00968 unit_c[2] = 0.0;
00969 VUNITIZE(unit_c);
00970 unit_h[0] = hx;
00971 unit_h[1] = hy;
00972 unit_h[2] = 0.0;
00973 VUNITIZE(unit_h);
00974
00975 if (fabs(VDOT(unit_d, unit_c)) >= tol->para)
00976 parallel = 1;
00977 else
00978 parallel = 0;
00979
00980 if (fabs(VDOT(unit_h, unit_c)) >= tol->para)
00981 parallel1 = 1;
00982 else
00983 parallel1 = 0;
00984
00985
00986
00987
00988
00989
00990 #define DETERMINANT_TOL 1.0e-14
00991 if (parallel || NEAR_ZERO(det, DETERMINANT_TOL)) {
00992
00993 if (!parallel1 && !NEAR_ZERO(det1, DETERMINANT_TOL)) {
00994
00995 if (bu_debug & BU_DEBUG_MATH) {
00996 bu_log("\tparallel, not co-linear. det=%e, det1=%g\n", det, det1);
00997 }
00998 return -1;
00999 }
01000
01001
01002
01003
01004
01005
01006
01007
01008 if (fabs(d[X]) >= fabs(d[Y])) {
01009 dist[0] = hx/d[X];
01010 dist[1] = (hx + c[X]) / d[X];
01011 } else {
01012 dist[0] = hy/d[Y];
01013 dist[1] = (hy + c[Y]) / d[Y];
01014 }
01015 if (bu_debug & BU_DEBUG_MATH) {
01016 bu_log("\tcolinear, t = %g, u = %g\n", dist[0], dist[1]);
01017 }
01018 return 0;
01019 }
01020 if (bu_debug & BU_DEBUG_MATH) {
01021
01022 bu_log("\thx=%g, hy=%g, det=%g, det1=%g, det2=%g\n", hx, hy, det, det1, (d[X] * hy - hx * d[Y]));
01023 }
01024 det = 1/det;
01025 dist[0] = det * det1;
01026 dist[1] = det * (d[X] * hy - hx * d[Y]);
01027 if (bu_debug & BU_DEBUG_MATH) {
01028 bu_log("\tintersection, t = %g, u = %g\n", dist[0], dist[1]);
01029 }
01030
01031 return 1;
01032 }
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072 int
01073 bn_isect_line2_lseg2(fastf_t *dist,
01074 const fastf_t *p,
01075 const fastf_t *d,
01076 const fastf_t *a,
01077 const fastf_t *c,
01078 const struct bn_tol *tol)
01079 {
01080 register fastf_t f;
01081 fastf_t ctol;
01082 int ret;
01083 point_t b;
01084
01085 BN_CK_TOL(tol);
01086 if (bu_debug & BU_DEBUG_MATH) {
01087 bu_log("bn_isect_line2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\t\ta=(%g, %g), adir=(%g, %g)\n",
01088 V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
01089 }
01090
01091
01092
01093
01094
01095
01096 if ((ctol = MAGSQ_2D(c)) <= tol->dist_sq) {
01097 ret = -4;
01098 goto out;
01099 }
01100
01101
01102
01103
01104
01105
01106 VADD2_2D(b, a, c);
01107 if (bn_distsq_line2_point2(p, d, a) <= tol->dist_sq &&
01108 (ctol=bn_distsq_line2_point2(p, d, b)) <= tol->dist_sq) {
01109 if (bu_debug & BU_DEBUG_MATH) {
01110 bu_log("b=(%g, %g), b_dist_sq=%g\n", V2ARGS(b), ctol);
01111 bu_log("bn_isect_line2_lseg2() pts A and B within tol of line\n");
01112 }
01113
01114 dist[0] = bn_dist_pt2_along_line2(p, d, a);
01115 dist[1] = bn_dist_pt2_along_line2(p, d, b);
01116 ret = 0;
01117 goto out;
01118 }
01119
01120 if ((ret = bn_isect_line2_line2(dist, p, d, a, c, tol)) < 0) {
01121
01122 ret = -3;
01123 goto out;
01124 }
01125 if (ret == 0) {
01126 fastf_t dtol;
01127
01128
01129 dtol = tol->dist / sqrt(MAGSQ_2D(d));
01130 if (bu_debug & BU_DEBUG_MATH) {
01131 bu_log("bn_isect_line2_lseg2() dtol=%g, dist[0]=%g, dist[1]=%g\n",
01132 dtol, dist[0], dist[1]);
01133 }
01134 if (dist[0] > -dtol && dist[0] < dtol) dist[0] = 0;
01135 else if (dist[0] > 1-dtol && dist[0] < 1+dtol) dist[0] = 1;
01136
01137 if (dist[1] > -dtol && dist[1] < dtol) dist[1] = 0;
01138 else if (dist[1] > 1-dtol && dist[1] < 1+dtol) dist[1] = 1;
01139 ret = 0;
01140 goto out;
01141 }
01142
01143
01144
01145
01146
01147
01148
01149
01150 {
01151 fastf_t ab_dist = 0;
01152 point_t hit_pt;
01153 point_t hit2;
01154
01155 VJOIN1_2D(hit_pt, p, dist[0], d);
01156 VJOIN1_2D(hit2, a, dist[1], c);
01157
01158 if (bn_pt2_pt2_equal(a, hit_pt, tol) ||
01159 bn_pt2_pt2_equal(a, hit2, tol)) {
01160 dist[1] = 0;
01161 ret = 1;
01162 }
01163 if (bn_pt2_pt2_equal(b, hit_pt, tol) ||
01164 bn_pt2_pt2_equal(b, hit_pt, tol)) {
01165 dist[1] = 1;
01166 ret = 2;
01167 }
01168
01169 ret = bn_isect_pt2_lseg2(&ab_dist, a, b, hit_pt, tol);
01170 if (bu_debug & BU_DEBUG_MATH) {
01171
01172 V2PRINT("a", a);
01173 V2PRINT("hit", hit_pt);
01174 V2PRINT("b", b);
01175 bu_log("bn_isect_pt2_lseg2() hit2d=(%g, %g) ab_dist=%g, ret=%d\n", hit_pt[X], hit_pt[Y], ab_dist, ret);
01176 bu_log("\tother hit2d=(%g, %g)\n", hit2[X], hit2[Y]);
01177 }
01178 if (ret <= 0) {
01179 if (ab_dist < 0) {
01180 ret = -2;
01181 } else {
01182 ret = -1;
01183 }
01184 goto out;
01185 }
01186 if (ret == 1) {
01187 dist[1] = 0;
01188 ret = 1;
01189 goto out;
01190 }
01191 if (ret == 2) {
01192 dist[1] = 1;
01193 ret = 2;
01194 goto out;
01195 }
01196
01197
01198 if (!bn_between(a[X], hit_pt[X], b[X], tol) ||
01199 !bn_between(a[Y], hit_pt[Y], b[Y], tol)) {
01200 bu_bomb("bn_isect_line2_lseg2() hit_pt not between A and B!\n");
01201 }
01202 }
01203
01204
01205
01206
01207
01208
01209
01210
01211 ctol = tol->dist / sqrt(ctol);
01212 if (bu_debug & BU_DEBUG_MATH) {
01213 bu_log("bn_isect_line2_lseg2() ctol=%g, dist[1]=%g\n", ctol, dist[1]);
01214 }
01215 if (dist[1] < -ctol) {
01216 ret = -2;
01217 goto out;
01218 }
01219 if ((f=(dist[1]-1)) > ctol) {
01220 ret = -1;
01221 goto out;
01222 }
01223
01224
01225 if (dist[1] < ctol) {
01226 dist[1] = 0;
01227 ret = 1;
01228 goto out;
01229 }
01230 if (f >= -ctol) {
01231 dist[1] = 1;
01232 ret = 2;
01233 goto out;
01234 }
01235 ret = 3;
01236 out:
01237 if (bu_debug & BU_DEBUG_MATH) {
01238 bu_log("bn_isect_line2_lseg2() dist[0]=%g, dist[1]=%g, ret=%d\n",
01239 dist[0], dist[1], ret);
01240 }
01241 return ret;
01242 }
01243
01244
01245
01246
01247
01248
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273
01274
01275
01276 int
01277 bn_isect_lseg2_lseg2(fastf_t *dist,
01278 const fastf_t *p,
01279 const fastf_t *pdir,
01280 const fastf_t *q,
01281 const fastf_t *qdir,
01282 const struct bn_tol *tol)
01283 {
01284 fastf_t ptol, qtol;
01285 int status;
01286
01287 BN_CK_TOL(tol);
01288 if (bu_debug & BU_DEBUG_MATH) {
01289 bu_log("bn_isect_lseg2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n",
01290 V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir));
01291 }
01292
01293 status = bn_isect_line2_line2(dist, p, pdir, q, qdir, tol);
01294 if (status < 0) {
01295
01296 return -1;
01297 }
01298 if (status == 0) {
01299 int nogood = 0;
01300
01301
01302 ptol = tol->dist / sqrt(MAGSQ_2D(pdir));
01303 if (bu_debug & BU_DEBUG_MATH) {
01304 bu_log("ptol=%g\n", ptol);
01305 }
01306 if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0;
01307 else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1;
01308
01309 if (dist[1] > -ptol && dist[1] < ptol) dist[1] = 0;
01310 else if (dist[1] > 1-ptol && dist[1] < 1+ptol) dist[1] = 1;
01311
01312 if (dist[1] < 0 || dist[1] > 1) nogood = 1;
01313 if (dist[0] < 0 || dist[0] > 1) nogood++;
01314 if (nogood >= 2)
01315 return -1;
01316 if (bu_debug & BU_DEBUG_MATH) {
01317 bu_log(" HIT colinear!\n");
01318 }
01319 return 0;
01320 }
01321
01322
01323 ptol = tol->dist / sqrt(MAGSQ_2D(pdir));
01324 if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0;
01325 else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1;
01326
01327 qtol = tol->dist / sqrt(MAGSQ_2D(qdir));
01328 if (dist[1] > -qtol && dist[1] < qtol) dist[1] = 0;
01329 else if (dist[1] > 1-qtol && dist[1] < 1+qtol) dist[1] = 1;
01330
01331 if (bu_debug & BU_DEBUG_MATH) {
01332 bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
01333 }
01334 if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) {
01335 if (bu_debug & BU_DEBUG_MATH) {
01336 bu_log(" MISS\n");
01337 }
01338 return -1;
01339 }
01340 if (bu_debug & BU_DEBUG_MATH) {
01341 bu_log(" HIT!\n");
01342 }
01343 return 1;
01344 }
01345
01346
01347
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361
01362
01363
01364
01365
01366
01367
01368
01369
01370
01371
01372
01373
01374
01375
01376
01377
01378
01379
01380
01381
01382
01383
01384
01385
01386
01387 int
01388 bn_isect_lseg3_lseg3(fastf_t *dist,
01389 const fastf_t *p,
01390 const fastf_t *pdir,
01391 const fastf_t *q,
01392 const fastf_t *qdir,
01393 const struct bn_tol *tol)
01394 {
01395 fastf_t ptol, qtol;
01396 fastf_t pmag, qmag;
01397 int status;
01398
01399 BN_CK_TOL(tol);
01400 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01401 bu_log("bn_isect_lseg3_lseg3() p=(%g, %g, %g), pdir=(%g, %g, %g)\n\t\tq=(%g, %g, %g), qdir=(%g, %g, %g)\n",
01402 V3ARGS(p), V3ARGS(pdir), V3ARGS(q), V3ARGS(qdir));
01403 }
01404
01405 status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol);
01406
01407
01408
01409
01410
01411
01412
01413
01414 if (UNLIKELY(status < -2 || status > 1)) {
01415 bu_bomb("bn_isect_lseg3_lseg3() function 'bn_isect_line3_line3' returned an invalid status\n");
01416 }
01417
01418 if (status == -1) {
01419
01420
01421
01422
01423 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01424 bu_log("bn_isect_lseg3_lseg3(): MISS, line segments do not intersect and are not parallel\n");
01425 }
01426 return -3;
01427 }
01428
01429 if (status == -2) {
01430
01431 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01432 bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are parallel, i.e. do not intersect\n");
01433 }
01434 return -2;
01435 }
01436
01437 pmag = MAGNITUDE(pdir);
01438 qmag = MAGNITUDE(qdir);
01439
01440 if (UNLIKELY(pmag < SMALL_FASTF)) {
01441 bu_bomb("bn_isect_lseg3_lseg3(): |p|=0\n");
01442 }
01443
01444 if (UNLIKELY(qmag < SMALL_FASTF)) {
01445 bu_bomb("bn_isect_lseg3_lseg3(): |q|=0\n");
01446 }
01447
01448 ptol = tol->dist / pmag;
01449 qtol = tol->dist / qmag;
01450 dist[0] = dist[0] / pmag;
01451 if (status == 0) {
01452
01453
01454
01455
01456 dist[1] = dist[1] / pmag;
01457 } else {
01458 dist[1] = dist[1] / qmag;
01459 }
01460
01461 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01462 bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
01463 }
01464
01465
01466 if (dist[0] > -ptol && dist[0] < ptol) {
01467 dist[0] = 0.0;
01468 } else if (dist[0] > 1.0-ptol && dist[0] < 1.0+ptol) {
01469 dist[0] = 1.0;
01470 }
01471
01472 if (status == 0) {
01473
01474
01475
01476
01477
01478 if (dist[1] > -ptol && dist[1] < ptol) {
01479 dist[1] = 0.0;
01480 } else if (dist[1] > 1.0-ptol && dist[1] < 1.0+ptol) {
01481 dist[1] = 1.0;
01482 }
01483 } else {
01484
01485 if (dist[1] > -qtol && dist[1] < qtol) {
01486 dist[1] = 0.0;
01487 } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) {
01488 dist[1] = 1.0;
01489 }
01490 }
01491
01492 if (status == 0) {
01493
01494 if ((dist[0] > 1.0+ptol && dist[1] > 1.0+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) {
01495 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01496 bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are colinear but not overlapping!\n");
01497 }
01498 return -1;
01499 }
01500
01501 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01502 bu_log("bn_isect_lseg3_lseg3(): HIT, line segments are colinear and overlapping!\n");
01503 }
01504
01505 return 0;
01506 }
01507
01508
01509
01510
01511 if (dist[0] < -ptol || dist[0] > 1.0+ptol || dist[1] < -qtol || dist[1] > 1.0+qtol) {
01512 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01513 bu_log("bn_isect_lseg3_lseg3(): MISS, infinite lines intersect but line segments do not!\n");
01514 }
01515 return -3;
01516 }
01517
01518 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01519 bu_log("bn_isect_lseg3_lseg3(): HIT, line segments intersect!\n");
01520 }
01521
01522
01523 if (UNLIKELY(dist[0] < -SMALL_FASTF || dist[0] > 1.0 || dist[1] < -SMALL_FASTF || dist[1] > 1.0)) {
01524 bu_bomb("bn_isect_lseg3_lseg3(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n");
01525 }
01526
01527 return 1;
01528 }
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573 int
01574 bn_isect_line3_line3(fastf_t *pdist,
01575 fastf_t *qdist,
01576 const fastf_t *p0,
01577 const fastf_t *pdir_i,
01578 const fastf_t *q0,
01579 const fastf_t *qdir_i,
01580 const struct bn_tol *tol)
01581 {
01582 fastf_t b, d, e, sc, tc, sc_numerator, tc_numerator, denominator;
01583 vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1;
01584 point_t p1, q1;
01585 fastf_t pdir_mag_sq;
01586 fastf_t qdir_mag_sq;
01587
01588 int parallel = 0;
01589 int colinear = 0;
01590 fastf_t dot, d1, d2, d3, d4;
01591 vect_t pdir, qdir;
01592
01593 VMOVE(pdir, pdir_i);
01594 VMOVE(qdir, qdir_i);
01595
01596 pdir_mag_sq = MAGSQ(pdir);
01597 qdir_mag_sq = MAGSQ(qdir);
01598
01599 if (UNLIKELY((pdir_mag_sq < tol->dist_sq) || (qdir_mag_sq < tol->dist_sq))) {
01600 bu_log(" p0 = %g %g %g\n", V3ARGS(p0));
01601 bu_log("pdir = %g %g %g\n", V3ARGS(pdir));
01602 bu_log(" q0 = %g %g %g\n", V3ARGS(q0));
01603 bu_log("qdir = %g %g %g\n", V3ARGS(qdir));
01604 bu_bomb("bn_isect_line3_line3(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n");
01605 }
01606
01607 *pdist = 0.0;
01608 *qdist = 0.0;
01609
01610
01611 VADD2(p1, p0, pdir);
01612 VADD2(q1, q0, qdir);
01613
01614 VSUB2(p0_to_q1, q1, p0);
01615
01616 d1 = bn_distsq_line3_pt3(q0,qdir,p0);
01617 d2 = bn_distsq_line3_pt3(q0,qdir,p1);
01618 d3 = bn_distsq_line3_pt3(p0,pdir,q0);
01619 d4 = bn_distsq_line3_pt3(p0,pdir,q1);
01620
01621
01622
01623
01624 if (NEAR_EQUAL(d1, d2, tol->dist_sq) &&
01625 NEAR_EQUAL(d1, d3, tol->dist_sq) &&
01626 NEAR_EQUAL(d1, d4, tol->dist_sq) &&
01627 NEAR_EQUAL(d2, d3, tol->dist_sq) &&
01628 NEAR_EQUAL(d2, d4, tol->dist_sq) &&
01629 NEAR_EQUAL(d3, d4, tol->dist_sq)) {
01630 parallel = 1;
01631 }
01632
01633 if (NEAR_ZERO(d1, tol->dist_sq) &&
01634 NEAR_ZERO(d2, tol->dist_sq) &&
01635 NEAR_ZERO(d3, tol->dist_sq) &&
01636 NEAR_ZERO(d4, tol->dist_sq)) {
01637 colinear = 1;
01638 }
01639
01640 VSUB2(w0, p0, q0);
01641 b = VDOT(pdir, qdir);
01642 d = VDOT(pdir, w0);
01643 e = VDOT(qdir, w0);
01644 denominator = pdir_mag_sq * qdir_mag_sq - b * b;
01645
01646 if (UNLIKELY(!parallel && colinear)) {
01647 bu_bomb("bn_isect_line3_line3(): logic error, lines colinear but not parallel\n");
01648 }
01649
01650 if (parallel && !colinear) {
01651
01652 sc = d / pdir_mag_sq;
01653 tc = 0.0;
01654 return -2;
01655 }
01656
01657 if (parallel && colinear) {
01658
01659
01660
01661
01662 *pdist = MAGNITUDE(w0);
01663 dot = VDOT(pdir, w0);
01664 if (dot > SMALL_FASTF) {
01665 *pdist = -(*pdist);
01666 }
01667
01668
01669
01670
01671 *qdist = MAGNITUDE(p0_to_q1);
01672
01673
01674
01675
01676 dot = VDOT(pdir, p0_to_q1);
01677 if (dot < -SMALL_FASTF) {
01678 *qdist = -(*qdist);
01679 }
01680
01681 return 0;
01682 }
01683
01684 sc_numerator = (b * e - qdir_mag_sq * d);
01685 tc_numerator = (pdir_mag_sq * e - b * d);
01686 sc = sc_numerator / denominator;
01687 tc = tc_numerator / denominator;
01688
01689 VSCALE(u_scaled, pdir, sc_numerator);
01690 VSCALE(v_scaled, qdir, tc_numerator);
01691 VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled);
01692 VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator);
01693 VADD2(qc_to_pc, w0, tmp_vec);
01694
01695 if (MAGSQ(qc_to_pc) <= tol->dist_sq) {
01696 *pdist = sc * sqrt(pdir_mag_sq);
01697 *qdist = tc * sqrt(qdir_mag_sq);
01698 return 1;
01699 } else {
01700 return -1;
01701 }
01702 }
01703
01704
01705
01706
01707
01708
01709
01710
01711
01712
01713
01714
01715
01716
01717
01718
01719
01720
01721
01722
01723
01724
01725
01726
01727
01728
01729
01730
01731
01732
01733
01734
01735
01736
01737 int
01738 bn_isect_line_lseg(fastf_t *t, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
01739 {
01740 vect_t ab, pa, pb;
01741 fastf_t ab_mag;
01742 fastf_t pa_mag_sq;
01743 fastf_t pb_mag_sq;
01744 fastf_t d_mag_sq;
01745 int code;
01746 fastf_t dist1, dist2, d1, d2;
01747 int colinear = 0;
01748 fastf_t dot;
01749
01750 BN_CK_TOL(tol);
01751
01752 *t = 0.0;
01753
01754 d_mag_sq = MAGSQ(d);
01755 if (UNLIKELY(NEAR_ZERO(d_mag_sq, tol->dist_sq))) {
01756 bu_bomb("bn_isect_line_lseg(): ray direction vector zero magnitude\n");
01757 }
01758
01759 VSUB2(ab, b, a);
01760 ab_mag = MAGNITUDE(ab);
01761 if (ab_mag < tol->dist) {
01762
01763 return -4;
01764 }
01765
01766 VSUB2(pa, a, p);
01767 pa_mag_sq = MAGSQ(pa);
01768 if (pa_mag_sq < tol->dist_sq) {
01769
01770 *t = sqrt(pa_mag_sq);
01771 return 1;
01772 }
01773
01774 VSUB2(pb, b, p);
01775 pb_mag_sq = MAGSQ(pb);
01776 if (pb_mag_sq < tol->dist_sq) {
01777
01778 *t = sqrt(pb_mag_sq);
01779 return 2;
01780 }
01781
01782
01783
01784
01785
01786
01787
01788 d1 = bn_distsq_line3_pt3(p,d,a);
01789 d2 = bn_distsq_line3_pt3(p,d,b);
01790
01791 colinear = 0;
01792 if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq)) {
01793 colinear = 1;
01794
01795 dist1 = sqrt(pa_mag_sq);
01796 dist2 = sqrt(pb_mag_sq);
01797
01798
01799
01800
01801
01802 dot = VDOT(pa, d);
01803 if (dot < -SMALL_FASTF) {
01804 dist1 = -dist1;
01805 }
01806
01807
01808
01809
01810
01811 dot = VDOT(pb, d);
01812 if (dot < -SMALL_FASTF) {
01813 dist2 = -dist2;
01814 }
01815 }
01816
01817 if (colinear && dist1 < SMALL_FASTF && dist2 < SMALL_FASTF) {
01818
01819 return -1;
01820 }
01821
01822 if (colinear && (dist1 > SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
01823
01824
01825 if (dist2 > dist1) {
01826 *t = dist1;
01827 } else {
01828 *t = dist2;
01829 }
01830 return 0;
01831 }
01832
01833 if (colinear && (dist1 > SMALL_FASTF) && (dist2 < SMALL_FASTF)) {
01834
01835
01836 *t = dist1;
01837 return 0;
01838 }
01839
01840 if (colinear && (dist1 < SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
01841
01842
01843 *t = dist2;
01844 return 0;
01845 }
01846
01847 dist1 = 0.0;
01848 dist2 = 0.0;
01849 code = bn_isect_line3_line3(&dist1, &dist2, p, d, a, ab, tol);
01850
01851 if (UNLIKELY(code == 0)) {
01852 bu_bomb("bn_isect_line_lseg(): we should have already detected a colinear condition\n");
01853 }
01854
01855 if (code < 0) {
01856 return -1;
01857 }
01858
01859 if (code == 1) {
01860 if (dist1 < -(tol->dist)) {
01861
01862
01863
01864 return -1;
01865 }
01866 }
01867
01868 if (code == 1) {
01869
01870 vect_t d_unit;
01871 vect_t a_to_isect_pt, b_to_isect_pt;
01872 point_t isect_pt;
01873 fastf_t a_to_isect_pt_mag_sq, b_to_isect_pt_mag_sq;
01874
01875 VMOVE(d_unit, d);
01876 VUNITIZE(d_unit);
01877
01878 dist1 = fabs(dist1);
01879 VSCALE(isect_pt, d_unit, dist1);
01880 VSUB2(a_to_isect_pt, isect_pt, a);
01881 VSUB2(b_to_isect_pt, isect_pt, b);
01882
01883 a_to_isect_pt_mag_sq = MAGSQ(a_to_isect_pt);
01884 b_to_isect_pt_mag_sq = MAGSQ(b_to_isect_pt);
01885
01886 *t = dist1;
01887
01888 if (a_to_isect_pt_mag_sq < tol->dist_sq) {
01889
01890 return 1;
01891 }
01892
01893 if (b_to_isect_pt_mag_sq < tol->dist_sq) {
01894
01895 return 2;
01896 }
01897
01898 if (UNLIKELY((a_to_isect_pt_mag_sq < tol->dist_sq) && (b_to_isect_pt_mag_sq < tol->dist_sq))) {
01899 bu_bomb("bn_isect_line_lseg(): this case should already been caught. i.e. zero length line segment\n");
01900 }
01901
01902 dot = VDOT(a_to_isect_pt, ab);
01903 if (dot < -SMALL_FASTF) {
01904
01905
01906
01907 return -3;
01908 }
01909
01910 dot = VDOT(b_to_isect_pt, ab);
01911 if (dot > SMALL_FASTF) {
01912
01913
01914
01915 return -2;
01916 }
01917
01918 return 3;
01919
01920
01921 }
01922
01923 bu_bomb("bn_isect_line_lseg(): logic error, should not be here\n");
01924
01925 return 0;
01926 }
01927
01928
01929
01930
01931
01932
01933
01934
01935
01936
01937
01938
01939
01940
01941
01942
01943
01944
01945 double
01946 bn_dist_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
01947 {
01948 vect_t f;
01949 register fastf_t FdotD;
01950
01951 if ((FdotD = MAGNITUDE(dir)) <= SMALL_FASTF) {
01952 FdotD = 0.0;
01953 goto out;
01954 }
01955 VSUB2(f, a, pt);
01956 FdotD = VDOT(f, dir) / FdotD;
01957 FdotD = MAGSQ(f) - FdotD * FdotD;
01958 if (FdotD <= SMALL_FASTF) {
01959 FdotD = 0.0;
01960 goto out;
01961 }
01962 FdotD = sqrt(FdotD);
01963 out:
01964 if (bu_debug & BU_DEBUG_MATH) {
01965 bu_log("bn_dist_line3_pt3() ret=%g\n", FdotD);
01966 }
01967 return FdotD;
01968 }
01969
01970
01971
01972
01973
01974
01975
01976
01977
01978
01979
01980
01981
01982
01983 double
01984 bn_distsq_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
01985 {
01986 vect_t f;
01987 register fastf_t FdotD;
01988
01989 VSUB2(f, pt, a);
01990 FdotD = MAGNITUDE(dir);
01991 if (ZERO(FdotD)) {
01992 FdotD = 0.0;
01993 goto out;
01994 }
01995 FdotD = VDOT(f, dir) / FdotD;
01996 FdotD = VDOT(f, f) - FdotD * FdotD;
01997 if (FdotD < SMALL_FASTF) {
01998 FdotD = 0.0;
01999 }
02000 out:
02001 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02002 bu_log("bn_distsq_line3_pt3() ret=%g\n", FdotD);
02003 }
02004 return FdotD;
02005 }
02006
02007
02008
02009
02010
02011
02012
02013
02014
02015
02016
02017
02018 double
02019 bn_dist_line_origin(const fastf_t *pt, const fastf_t *dir)
02020 {
02021 register fastf_t PTdotD;
02022
02023 if ((PTdotD = MAGNITUDE(dir)) <= SMALL_FASTF)
02024 return 0.0;
02025 PTdotD = VDOT(pt, dir) / PTdotD;
02026 if ((PTdotD = VDOT(pt, pt) - PTdotD * PTdotD) <= SMALL_FASTF)
02027 return 0.0;
02028 return sqrt(PTdotD);
02029 }
02030
02031
02032
02033
02034
02035
02036
02037
02038
02039
02040
02041
02042 double
02043 bn_dist_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
02044 {
02045 vect_t f;
02046 register fastf_t FdotD;
02047
02048 VSUB2_2D(f, pt, a);
02049 if ((FdotD = sqrt(MAGSQ_2D(dir))) <= SMALL_FASTF)
02050 return 0.0;
02051 FdotD = VDOT_2D(f, dir) / FdotD;
02052 if ((FdotD = VDOT_2D(f, f) - FdotD * FdotD) <= SMALL_FASTF)
02053 return 0.0;
02054 return sqrt(FdotD);
02055 }
02056
02057
02058
02059
02060
02061
02062
02063
02064
02065
02066
02067
02068
02069
02070 double
02071 bn_distsq_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
02072 {
02073 vect_t f;
02074 register fastf_t FdotD;
02075
02076 VSUB2_2D(f, pt, a);
02077 if ((FdotD = sqrt(MAGSQ_2D(dir))) <= SMALL_FASTF)
02078 return 0.0;
02079 FdotD = VDOT_2D(f, dir) / FdotD;
02080 if ((FdotD = VDOT_2D(f, f) - FdotD * FdotD) <= SMALL_FASTF)
02081 return 0.0;
02082 return FdotD;
02083 }
02084
02085
02086
02087
02088
02089
02090
02091 double
02092 bn_area_of_triangle(register const fastf_t *a, register const fastf_t *b, register const fastf_t *c)
02093 {
02094 register double t;
02095 register double area;
02096
02097 t = a[Y] * (b[Z] - c[Z]) -
02098 b[Y] * (a[Z] - c[Z]) +
02099 c[Y] * (a[Z] - b[Z]);
02100 area = t*t;
02101 t = a[Z] * (b[X] - c[X]) -
02102 b[Z] * (a[X] - c[X]) +
02103 c[Z] * (a[X] - b[X]);
02104 area += t*t;
02105 t = a[X] * (b[Y] - c[Y]) -
02106 b[X] * (a[Y] - c[Y]) +
02107 c[X] * (a[Y] - b[Y]);
02108 area += t*t;
02109
02110 return 0.5 * sqrt(area);
02111 }
02112
02113
02114
02115
02116
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127
02128
02129
02130
02131
02132
02133
02134
02135
02136
02137
02138
02139
02140
02141
02142
02143
02144
02145
02146
02147 int bn_isect_pt_lseg(fastf_t *dist,
02148 const fastf_t *a,
02149 const fastf_t *b,
02150 const fastf_t *p,
02151 const struct bn_tol *tol)
02152
02153
02154
02155 {
02156 vect_t AtoP, BtoP, AtoB,
02157 ABunit;
02158 fastf_t APprABunit;
02159 fastf_t distsq;
02160
02161 BN_CK_TOL(tol);
02162
02163 VSUB2(AtoP, p, a);
02164 if (MAGSQ(AtoP) < tol->dist_sq)
02165 return 1;
02166
02167 VSUB2(BtoP, p, b);
02168 if (MAGSQ(BtoP) < tol->dist_sq)
02169 return 2;
02170
02171 VSUB2(AtoB, b, a);
02172 VMOVE(ABunit, AtoB);
02173 distsq = MAGSQ(ABunit);
02174 if (distsq < tol->dist_sq)
02175 return -1;
02176 distsq = 1/sqrt(distsq);
02177 VSCALE(ABunit, ABunit, distsq);
02178
02179
02180
02181
02182
02183
02184
02185
02186 APprABunit = VDOT(AtoP, ABunit);
02187
02188
02189 distsq = MAGSQ(AtoP) - APprABunit * APprABunit;
02190 if (distsq > tol->dist_sq)
02191 return -1;
02192
02193
02194 *dist = VDOT(AtoP, AtoB) / MAGSQ(AtoB);
02195
02196 if (*dist > 1.0 || *dist < -SMALL_FASTF)
02197 return -2;
02198
02199 return 3;
02200 }
02201
02202
02203
02204
02205
02206
02207
02208
02209
02210
02211
02212
02213
02214
02215
02216
02217
02218
02219
02220
02221
02222
02223
02224
02225
02226
02227
02228
02229
02230 int
02231 bn_isect_pt2_lseg2(fastf_t *dist, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
02232
02233
02234
02235 {
02236 vect_t AtoP,
02237 BtoP,
02238 AtoB,
02239 ABunit;
02240 fastf_t APprABunit;
02241 fastf_t distsq;
02242
02243 BN_CK_TOL(tol);
02244
02245 VSUB2_2D(AtoP, p, a);
02246 if (MAGSQ_2D(AtoP) < tol->dist_sq)
02247 return 1;
02248
02249 VSUB2_2D(BtoP, p, b);
02250 if (MAGSQ_2D(BtoP) < tol->dist_sq)
02251 return 2;
02252
02253 VSUB2_2D(AtoB, b, a);
02254 VMOVE_2D(ABunit, AtoB);
02255 distsq = MAGSQ_2D(ABunit);
02256 if (distsq < tol->dist_sq) {
02257 if (bu_debug & BU_DEBUG_MATH) {
02258 bu_log("distsq A=%g\n", distsq);
02259 }
02260 return -1;
02261 }
02262 distsq = 1/sqrt(distsq);
02263 VSCALE_2D(ABunit, ABunit, distsq);
02264
02265
02266
02267
02268
02269
02270
02271
02272 APprABunit = VDOT_2D(AtoP, ABunit);
02273
02274
02275 distsq = MAGSQ_2D(AtoP) - APprABunit * APprABunit;
02276 if (distsq > tol->dist_sq) {
02277 if (bu_debug & BU_DEBUG_MATH) {
02278 V2PRINT("ABunit", ABunit);
02279 bu_log("distsq B=%g\n", distsq);
02280 }
02281 return -1;
02282 }
02283
02284
02285 *dist = VDOT_2D(AtoP, AtoB) / MAGSQ_2D(AtoB);
02286
02287 if (*dist > 1.0 || *dist < 0.0)
02288 return -2;
02289
02290 return 3;
02291 }
02292
02293
02294
02295
02296
02297
02298
02299
02300
02301
02302
02303
02304
02305
02306
02307
02308
02309
02310
02311
02312
02313
02314
02315
02316
02317
02318
02319
02320
02321
02322
02323
02324
02325
02326 int
02327 bn_dist_pt3_lseg3(fastf_t *dist,
02328 fastf_t *pca,
02329 const fastf_t *a,
02330 const fastf_t *b,
02331 const fastf_t *p,
02332 const struct bn_tol *tol)
02333 {
02334 vect_t PtoA;
02335 vect_t PtoB;
02336 vect_t AtoB;
02337 fastf_t P_A_sq;
02338 fastf_t P_B_sq;
02339 fastf_t B_A;
02340 fastf_t t;
02341
02342 BN_CK_TOL(tol);
02343
02344 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02345 bu_log("bn_dist_pt3_lseg3() a=(%g, %g, %g) b=(%g, %g, %g)\n\tp=(%g, %g, %g), tol->dist=%g sq=%g\n",
02346 V3ARGS(a),
02347 V3ARGS(b),
02348 V3ARGS(p),
02349 tol->dist, tol->dist_sq);
02350 }
02351
02352
02353 VSUB2(PtoA, p, a);
02354 if ((P_A_sq = MAGSQ(PtoA)) < tol->dist_sq) {
02355
02356 VMOVE(pca, a);
02357 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" at A\n");
02358 *dist = 0.0;
02359 return 1;
02360 }
02361
02362
02363 VSUB2(PtoB, p, b);
02364 if ((P_B_sq = MAGSQ(PtoB)) < tol->dist_sq) {
02365
02366 VMOVE(pca, b);
02367 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" at B\n");
02368 *dist = 0.0;
02369 return 2;
02370 }
02371
02372 VSUB2(AtoB, b, a);
02373 B_A = sqrt(MAGSQ(AtoB));
02374
02375
02376
02377
02378 t = VDOT(PtoA, AtoB) / B_A;
02379 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02380 bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
02381 B_A, t);
02382 }
02383
02384 if (t <= SMALL_FASTF) {
02385
02386 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" left of A\n");
02387 VMOVE(pca, a);
02388 *dist = sqrt(P_A_sq);
02389 return 3;
02390 }
02391 if (t < B_A) {
02392
02393 register fastf_t dsq;
02394 fastf_t param_dist;
02395
02396
02397 param_dist = t / B_A;
02398 VJOIN1(pca, a, param_dist, AtoB);
02399
02400
02401 if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
02402 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" ON lseg\n");
02403
02404 *dist = param_dist;
02405 return 0;
02406 }
02407 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" closest to lseg\n");
02408 *dist = sqrt(dsq);
02409 return 5;
02410 }
02411
02412 if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" right of B\n");
02413 VMOVE(pca, b);
02414 *dist = sqrt(P_B_sq);
02415 return 4;
02416 }
02417
02418
02419
02420
02421
02422
02423
02424
02425
02426
02427
02428
02429
02430
02431
02432
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448 int
02449 bn_dist_pt2_lseg2(fastf_t *dist_sq, fastf_t *pca, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
02450 {
02451 vect_t PtoA;
02452 vect_t PtoB;
02453 vect_t AtoB;
02454 fastf_t P_A_sq;
02455 fastf_t P_B_sq;
02456 fastf_t B_A;
02457 fastf_t t;
02458
02459 BN_CK_TOL(tol);
02460
02461 if (bu_debug & BU_DEBUG_MATH) {
02462 bu_log("bn_dist_pt3_lseg3() a=(%g, %g, %g) b=(%g, %g, %g)\n\tp=(%g, %g, %g), tol->dist=%g sq=%g\n",
02463 V3ARGS(a),
02464 V3ARGS(b),
02465 V3ARGS(p),
02466 tol->dist, tol->dist_sq);
02467 }
02468
02469
02470
02471 VSUB2_2D(PtoA, p, a);
02472 if ((P_A_sq = MAGSQ_2D(PtoA)) < tol->dist_sq) {
02473
02474 V2MOVE(pca, a);
02475 if (bu_debug & BU_DEBUG_MATH) bu_log(" at A\n");
02476 *dist_sq = 0.0;
02477 return 1;
02478 }
02479
02480
02481 VSUB2_2D(PtoB, p, b);
02482 if ((P_B_sq = MAGSQ_2D(PtoB)) < tol->dist_sq) {
02483
02484 V2MOVE(pca, b);
02485 if (bu_debug & BU_DEBUG_MATH) bu_log(" at B\n");
02486 *dist_sq = 0.0;
02487 return 2;
02488 }
02489
02490 VSUB2_2D(AtoB, b, a);
02491 B_A = sqrt(MAGSQ_2D(AtoB));
02492
02493
02494
02495
02496 t = VDOT_2D(PtoA, AtoB) / B_A;
02497 if (bu_debug & BU_DEBUG_MATH) {
02498 bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
02499 B_A, t);
02500 }
02501
02502 if (t <= 0) {
02503
02504 if (bu_debug & BU_DEBUG_MATH) bu_log(" left of A\n");
02505 V2MOVE(pca, a);
02506 *dist_sq = P_A_sq;
02507 return 3;
02508 }
02509 if (t < B_A) {
02510
02511 register fastf_t dsq;
02512 fastf_t param_dist;
02513
02514
02515 param_dist = t / B_A;
02516 V2JOIN1(pca, a, param_dist, AtoB);
02517
02518
02519 if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
02520 if (bu_debug & BU_DEBUG_MATH) bu_log(" ON lseg\n");
02521
02522 *dist_sq = param_dist;
02523 return 0;
02524 }
02525 if (bu_debug & BU_DEBUG_MATH) bu_log(" closest to lseg\n");
02526 *dist_sq = dsq;
02527 return 5;
02528 }
02529
02530 if (bu_debug & BU_DEBUG_MATH) bu_log(" right of B\n");
02531 V2MOVE(pca, b);
02532 *dist_sq = P_B_sq;
02533 return 4;
02534 }
02535
02536
02537
02538
02539
02540
02541
02542
02543
02544
02545 void
02546 bn_rotate_bbox(fastf_t *omin, fastf_t *omax, const fastf_t *mat, const fastf_t *imin, const fastf_t *imax)
02547 {
02548 point_t local;
02549 point_t model;
02550
02551 #define ROT_VERT(a, b, c) { \
02552 VSET(local, a[X], b[Y], c[Z]); \
02553 MAT4X3PNT(model, mat, local); \
02554 VMINMAX(omin, omax, model); \
02555 }
02556
02557 ROT_VERT(imin, imin, imin);
02558 ROT_VERT(imin, imin, imax);
02559 ROT_VERT(imin, imax, imin);
02560 ROT_VERT(imin, imax, imax);
02561 ROT_VERT(imax, imin, imin);
02562 ROT_VERT(imax, imin, imax);
02563 ROT_VERT(imax, imax, imin);
02564 ROT_VERT(imax, imax, imax);
02565 #undef ROT_VERT
02566 }
02567
02568
02569
02570
02571
02572
02573
02574 void
02575 bn_rotate_plane(fastf_t *oplane, const fastf_t *mat, const fastf_t *iplane)
02576 {
02577 point_t orig_pt;
02578 point_t new_pt;
02579
02580
02581 VSCALE(orig_pt, iplane, iplane[3]);
02582
02583
02584 MAT4X3VEC(oplane, mat, iplane);
02585
02586
02587 MAT4X3PNT(new_pt, mat, orig_pt);
02588
02589
02590
02591
02592
02593 oplane[3] = VDOT(new_pt, oplane);
02594 }
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609 int
02610 bn_coplanar(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
02611 {
02612 register fastf_t dot;
02613 vect_t pt_a, pt_b;
02614 BN_CK_TOL(tol);
02615
02616 if (!NEAR_EQUAL(MAGSQ(a), 1.0, VUNITIZE_TOL) || !NEAR_EQUAL(MAGSQ(b), 1.0, VUNITIZE_TOL)) {
02617 bu_bomb("bn_coplanar(): input vector(s) 'a' and/or 'b' is not a unit vector.\n");
02618 }
02619
02620 VSCALE(pt_a, a, fabs(a[W]));
02621 VSCALE(pt_b, b, fabs(b[W]));
02622 dot = VDOT(a, b);
02623
02624 if (NEAR_ZERO(dot, tol->perp)) {
02625 return 0;
02626 }
02627
02628
02629 if ((dot <= -SMALL_FASTF) ? (NEAR_EQUAL(dot, -1.0, tol->perp)) : (NEAR_EQUAL(dot, 1.0, tol->perp))) {
02630 if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) {
02631
02632 if (dot >= SMALL_FASTF) {
02633
02634 return 1;
02635 } else {
02636
02637 return 2;
02638 }
02639 } else {
02640 return -1;
02641 }
02642 }
02643 return 0;
02644 }
02645
02646
02647
02648
02649
02650
02651
02652
02653
02654
02655
02656
02657
02658
02659
02660
02661
02662
02663
02664
02665
02666
02667
02668
02669
02670
02671
02672
02673
02674
02675
02676
02677 double
02678 bn_angle_measure(fastf_t *vec, const fastf_t *x_dir, const fastf_t *y_dir)
02679 {
02680 fastf_t xproj, yproj;
02681 fastf_t gam;
02682 fastf_t ang;
02683
02684 xproj = -VDOT(vec, x_dir);
02685 yproj = -VDOT(vec, y_dir);
02686 gam = atan2(yproj, xproj);
02687 ang = bn_pi + gam;
02688 if (ang < -SMALL_FASTF) {
02689 do {
02690 ang += bn_twopi;
02691 } while (ang < -SMALL_FASTF);
02692 } else if (ang > bn_twopi) {
02693 do {
02694 ang -= bn_twopi;
02695 } while (ang > bn_twopi);
02696 }
02697 if (UNLIKELY(ang < -SMALL_FASTF || ang > bn_twopi))
02698 bu_bomb("bn_angle_measure() angle out of range\n");
02699
02700 return ang;
02701 }
02702
02703
02704
02705
02706
02707
02708
02709
02710
02711
02712 double
02713 bn_dist_pt3_along_line3(const fastf_t *p, const fastf_t *d, const fastf_t *x)
02714 {
02715 vect_t x_p;
02716
02717 VSUB2(x_p, x, p);
02718 return VDOT(x_p, d);
02719 }
02720
02721
02722
02723
02724
02725
02726
02727
02728
02729
02730 double
02731 bn_dist_pt2_along_line2(const fastf_t *p, const fastf_t *d, const fastf_t *x)
02732 {
02733 vect_t x_p;
02734 double ret;
02735
02736 VSUB2_2D(x_p, x, p);
02737 ret = VDOT_2D(x_p, d);
02738 if (bu_debug & BU_DEBUG_MATH) {
02739 bu_log("bn_dist_pt2_along_line2() p=(%g, %g), d=(%g, %g), x=(%g, %g) ret=%g\n",
02740 V2ARGS(p),
02741 V2ARGS(d),
02742 V2ARGS(x),
02743 ret);
02744 }
02745 return ret;
02746 }
02747
02748
02749
02750
02751
02752
02753
02754 int
02755 bn_between(double left, double mid, double right, const struct bn_tol *tol)
02756 {
02757 BN_CK_TOL(tol);
02758
02759 if (left < right) {
02760 if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
02761 left -= tol->dist*0.1;
02762 right += tol->dist*0.1;
02763 }
02764 if (mid < left || mid > right) goto fail;
02765 return 1;
02766 }
02767
02768 if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
02769 right -= tol->dist*0.1;
02770 left += tol->dist*0.1;
02771 }
02772 if (mid < right || mid > left) goto fail;
02773 return 1;
02774 fail:
02775 if (bu_debug & BU_DEBUG_MATH) {
02776 bu_log("bn_between(%.17e, %.17e, %.17e) ret=0 FAIL\n",
02777 left, mid, right);
02778 }
02779 return 0;
02780 }
02781
02782
02783
02784
02785
02786
02787
02788
02789 int
02790 bn_does_ray_isect_tri(
02791 const point_t pt,
02792 const vect_t dir,
02793 const point_t V,
02794 const point_t A,
02795 const point_t B,
02796 point_t inter)
02797 {
02798 vect_t VP, VA, VB, AB, AP, N;
02799 fastf_t NdotDir;
02800 plane_t pl;
02801 fastf_t dist;
02802
02803
02804
02805 VSUB2(VA, A, V);
02806 VSUB2(VB, B, V);
02807 VCROSS(pl, VA, VB);
02808 VUNITIZE(pl);
02809
02810 NdotDir = VDOT(pl, dir);
02811 if (ZERO(NdotDir))
02812 return 0;
02813
02814 pl[W] = VDOT(pl, V);
02815
02816 dist = (pl[W] - VDOT(pl, pt))/NdotDir;
02817 VJOIN1(inter, pt, dist, dir);
02818
02819
02820 VSUB2(VP, inter, V);
02821 VCROSS(N, VA, VP);
02822 if (VDOT(N, pl) < 0.0)
02823 return 0;
02824
02825 VCROSS(N, VP, VB);
02826 if (VDOT(N, pl) < 0.0)
02827 return 0;
02828
02829 VSUB2(AB, B, A);
02830 VSUB2(AP, inter, A);
02831 VCROSS(N, AB, AP);
02832 if (VDOT(N, pl) < 0.0)
02833 return 0;
02834
02835 return 1;
02836 }
02837
02838
02839 #if 0
02840
02841
02842
02843
02844
02845
02846
02847
02848
02849
02850
02851
02852
02853
02854
02855
02856
02857
02858
02859
02860
02861
02862
02863
02864
02865
02866
02867
02868
02869
02870
02871
02872
02873
02874
02875
02876
02877
02878
02879
02880
02881 int
02882 bn_isect_ray_tri(dist_p, N_p, Alpha_p, Beta_p, pt, dir, V, A, B)
02883 fastf_t *dist_p;
02884 fastf_t *N_p;
02885 fastf_t *Alpha_p;
02886 fastf_t *Beta_p;
02887 const point_t pt;
02888 const vect_t dir;
02889 const point_t V;
02890 const point_t A;
02891 const point_t B;
02892 {
02893 vect_t VA;
02894 vect_t VB;
02895 vect_t pt_V;
02896 vect_t N;
02897 vect_t VPP;
02898 fastf_t alpha;
02899 fastf_t beta;
02900 fastf_t NdotDir;
02901 fastf_t entleave;
02902 fastf_t k;
02903
02904
02905 VSUB2(VB, B, V);
02906 VSUB2(VA, A, V);
02907
02908
02909 VCROSS(N, VA, VB);
02910
02911 NdotDir = VDOT(N, dir);
02912 if (fabs(NdotDir) < SQRT_SMALL_FASTF)
02913 return 0;
02914
02915 if (NdotDir >= 0.0) entleave = -1;
02916 else entleave = 1;
02917
02918 VSUB2(pt_V, V, pt);
02919 VCROSS(VPP, pt_V, dir);
02920
02921
02922
02923
02924
02925 alpha = VDOT(VA, VPP) * entleave;
02926 if (alpha < 0.0 || alpha > fabs(NdotDir)) return 0;
02927
02928
02929
02930 beta = VDOT(VB, VPP) * (-1 * entleave);
02931 if (beta < 0.0 || beta > fabs(NdotDir)) return 0;
02932
02933 k = VDOT(VPP, N) / NdotDir;
02934
02935 if (dist_p) *dist_p = k;
02936 if (N_p) {
02937 VUNITIZE(N);
02938 VMOVE(N_p, N);
02939 }
02940 if (Alpha_p) *Alpha_p = alpha;
02941 if (Beta_p) *Beta_p = beta;
02942
02943 return entleave;
02944 }
02945 #endif
02946
02947
02948
02949
02950
02951
02952
02953
02954
02955
02956
02957 int
02958 bn_hlf_class(const fastf_t *half_eqn, const fastf_t *min, const fastf_t *max, const struct bn_tol *tol)
02959 {
02960 int class;
02961 fastf_t d;
02962
02963 #define CHECK_PT(x, y, z) \
02964 d = (x)*half_eqn[0] + (y)*half_eqn[1] + (z)*half_eqn[2] - half_eqn[3]; \
02965 if (d < -tol->dist) { \
02966 if (class == BN_CLASSIFY_OUTSIDE) \
02967 return BN_CLASSIFY_OVERLAPPING; \
02968 else class = BN_CLASSIFY_INSIDE; \
02969 } else if (d > tol->dist) { \
02970 if (class == BN_CLASSIFY_INSIDE) \
02971 return BN_CLASSIFY_OVERLAPPING; \
02972 else class = BN_CLASSIFY_OUTSIDE; \
02973 } else return BN_CLASSIFY_OVERLAPPING
02974
02975 class = BN_CLASSIFY_UNIMPLEMENTED;
02976 CHECK_PT(min[X], min[Y], min[Z]);
02977 CHECK_PT(min[X], min[Y], max[Z]);
02978 CHECK_PT(min[X], max[Y], min[Z]);
02979 CHECK_PT(min[X], max[Y], max[Z]);
02980 CHECK_PT(max[X], min[Y], min[Z]);
02981 CHECK_PT(max[X], min[Y], max[Z]);
02982 CHECK_PT(max[X], max[Y], min[Z]);
02983 CHECK_PT(max[X], max[Y], max[Z]);
02984 if (class == BN_CLASSIFY_UNIMPLEMENTED)
02985 bu_log("bn_hlf_class: error in implementation\
02986 min = (%g, %g, %g), max = (%g, %g, %g), half_eqn = (%g, %g, %g, %g)\n",
02987 V3ARGS(min), V3ARGS(max), V3ARGS(half_eqn),
02988 half_eqn[3]);
02989 return class;
02990 }
02991
02992
02993
02994
02995
02996
02997
02998
02999
03000
03001
03002
03003
03004
03005
03006
03007
03008
03009
03010
03011
03012
03013
03014
03015
03016
03017 int
03018 bn_distsq_line3_line3(fastf_t *dist, fastf_t *P, fastf_t *d_in, fastf_t *Q, fastf_t *e_in, fastf_t *pt1, fastf_t *pt2)
03019 {
03020 fastf_t de, denom;
03021 vect_t diff, PmQ, tmp;
03022 vect_t d, e;
03023 fastf_t len_e, inv_len_e, len_d, inv_len_d;
03024 int ret=0;
03025
03026 len_e = MAGNITUDE(e_in);
03027 if (ZERO(len_e))
03028 bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
03029 inv_len_e = 1.0 / len_e;
03030
03031 len_d = MAGNITUDE(d_in);
03032 if (ZERO(len_d))
03033 bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
03034 inv_len_d = 1.0 / len_d;
03035
03036 VSCALE(e, e_in, inv_len_e);
03037 VSCALE(d, d_in, inv_len_d);
03038 de = VDOT(d, e);
03039
03040 if (ZERO(de)) {
03041
03042 dist[0] = VDOT(Q, d) - VDOT(P, d);
03043 dist[1] = VDOT(P, e) - VDOT(Q, e);
03044 } else {
03045 VSUB2(PmQ, P, Q);
03046 denom = 1.0 - de*de;
03047 if (ZERO(denom)) {
03048
03049 dist[0] = 0.0;
03050 dist[1] = VDOT(PmQ, d);
03051 ret = 1;
03052 } else {
03053 VBLEND2(tmp, 1.0, e, -de, d);
03054 dist[1] = VDOT(PmQ, tmp)/denom;
03055 dist[0] = dist[1] * de - VDOT(PmQ, d);
03056 }
03057 }
03058 VJOIN1(pt1, P, dist[0], d);
03059 VJOIN1(pt2, Q, dist[1], e);
03060 VSUB2(diff, pt1, pt2);
03061 dist[0] *= inv_len_d;
03062 dist[1] *= inv_len_e;
03063 dist[2] = MAGSQ(diff);
03064 return ret;
03065 }
03066
03067
03068
03069
03070
03071
03072
03073
03074
03075
03076
03077
03078
03079
03080
03081
03082
03083
03084
03085
03086
03087
03088
03089
03090
03091
03092
03093
03094
03095
03096
03097 int
03098 bn_isect_planes(fastf_t *pt, const fastf_t (*planes)[4], const size_t pl_count)
03099 {
03100 mat_t matrix;
03101 mat_t inverse;
03102 vect_t hpq;
03103 fastf_t det;
03104 size_t i;
03105
03106 if (bu_debug & BU_DEBUG_MATH) {
03107 bu_log("bn_isect_planes:\n");
03108 for (i=0; i<pl_count; i++) {
03109 bu_log("Plane #%zu (%f %f %f %f)\n", i, V4ARGS(planes[i]));
03110 }
03111 }
03112
03113 MAT_ZERO(matrix);
03114 VSET(hpq, 0.0, 0.0, 0.0);
03115
03116 for (i=0; i<pl_count; i++) {
03117 matrix[0] += planes[i][X] * planes[i][X];
03118 matrix[5] += planes[i][Y] * planes[i][Y];
03119 matrix[10] += planes[i][Z] * planes[i][Z];
03120 matrix[1] += planes[i][X] * planes[i][Y];
03121 matrix[2] += planes[i][X] * planes[i][Z];
03122 matrix[6] += planes[i][Y] * planes[i][Z];
03123 hpq[X] += planes[i][X] * planes[i][H];
03124 hpq[Y] += planes[i][Y] * planes[i][H];
03125 hpq[Z] += planes[i][Z] * planes[i][H];
03126 }
03127
03128 matrix[4] = matrix[1];
03129 matrix[8] = matrix[2];
03130 matrix[9] = matrix[6];
03131 matrix[15] = 1.0;
03132
03133
03134 det = bn_mat_determinant(matrix);
03135 if (ZERO(det))
03136 return 1;
03137
03138 bn_mat_inv(inverse, matrix);
03139
03140 MAT4X3PNT(pt, inverse, hpq);
03141
03142 return 0;
03143
03144 }
03145
03146
03147
03148
03149
03150
03151
03152
03153
03154
03155
03156
03157
03158
03159
03160
03161
03162
03163
03164
03165 int
03166 bn_isect_lseg_rpp(fastf_t *a,
03167 fastf_t *b,
03168 register fastf_t *min,
03169 register fastf_t *max)
03170 {
03171 auto vect_t diff;
03172 register fastf_t *pt = &a[0];
03173 register fastf_t *dir = &diff[0];
03174 register int i;
03175 register double sv;
03176 register double st;
03177 register double mindist, maxdist;
03178
03179 mindist = -MAX_FASTF;
03180 maxdist = MAX_FASTF;
03181 VSUB2(diff, b, a);
03182
03183 for (i=0; i < 3; i++, pt++, dir++, max++, min++) {
03184 if (*dir < -SQRT_SMALL_FASTF) {
03185 if ((sv = (*min - *pt) / *dir) < 0.0)
03186 return 0;
03187 if (maxdist > sv)
03188 maxdist = sv;
03189 if (mindist < (st = (*max - *pt) / *dir))
03190 mindist = st;
03191 } else if (*dir > SQRT_SMALL_FASTF) {
03192 if ((st = (*max - *pt) / *dir) < 0.0)
03193 return 0;
03194 if (maxdist > st)
03195 maxdist = st;
03196 if (mindist < ((sv = (*min - *pt) / *dir)))
03197 mindist = sv;
03198 } else {
03199
03200
03201
03202
03203 if ((*min > *pt) || (*max < *pt))
03204 return 0; ;
03205 }
03206 }
03207 if (mindist >= maxdist)
03208 return 0;
03209
03210 if (mindist > 1 || maxdist < 0)
03211 return 0;
03212
03213 if (mindist >= 0 && maxdist <= 1)
03214 return 1;
03215
03216
03217 if (mindist < 0)
03218 mindist = 0;
03219 if (maxdist > 1)
03220 maxdist = 1;
03221
03222
03223 VJOIN1(b, a, maxdist, diff);
03224 VJOIN1(a, a, mindist, diff);
03225 return 1;
03226 }
03227
03228
03229
03230
03231
03232
03233
03234
03235
03236
03237
03238
03239