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 "IOstreams.H"
00030 #include "pointHit.H"
00031 #include "mathematicalConstants.H"
00032
00033
00034
00035 namespace Foam
00036 {
00037
00038
00039
00040 template<class Point, class PointRef>
00041 bool triangle<Point, PointRef>::intersection
00042 (
00043 const Point& baseVertex,
00044 const vector& E0,
00045 const vector& E1,
00046 const vector& n,
00047 const point& P,
00048 const vector& dir,
00049 point& pInter
00050 )
00051 {
00052
00053 scalar denom = n & dir;
00054
00055 if (Foam::mag(denom) < SMALL)
00056 {
00057
00058 pInter = P;
00059 return false;
00060 }
00061 pInter = P + dir*(n & (baseVertex - P))/denom;
00062
00063
00064 scalar magX = Foam::mag(n.x());
00065 scalar magY = Foam::mag(n.y());
00066 scalar magZ = Foam::mag(n.z());
00067
00068 label i0 = -1;
00069 if ((magX >= magY) && (magX >= magZ))
00070 {
00071 i0 = 0;
00072 }
00073 else if ((magY >= magX) && (magY >= magZ))
00074 {
00075 i0 = 1;
00076 }
00077 else
00078 {
00079 i0 = 2;
00080 }
00081
00082
00083 label i1 = (i0 + 1) % 3;
00084 label i2 = (i1 + 1) % 3;
00085
00086 scalar u1 = E0[i1];
00087 scalar v1 = E0[i2];
00088
00089 scalar u2 = E1[i1];
00090 scalar v2 = E1[i2];
00091
00092 scalar det = v2*u1 - u2*v1;
00093
00094 scalar u0 = pInter[i1] - baseVertex[i1];
00095 scalar v0 = pInter[i2] - baseVertex[i2];
00096
00097 scalar alpha = 0;
00098 scalar beta = 0;
00099 bool hit = false;
00100
00101 if (Foam::mag(u1) < SMALL)
00102 {
00103 beta = u0/u2;
00104 if ((beta >= 0) && (beta <= 1))
00105 {
00106 alpha = (v0 - beta*v2)/v1;
00107 hit = ((alpha >= 0) && ((alpha + beta) <= 1));
00108 }
00109 }
00110 else
00111 {
00112 beta = (v0*u1 - u0*v1)/det;
00113 if ((beta >= 0) && (beta <= 1))
00114 {
00115 alpha = (u0 - beta*u2)/u1;
00116 hit = ((alpha >= 0) && ((alpha + beta) <= 1));
00117 }
00118 }
00119
00120 return hit;
00121 }
00122
00123
00124 template<class Point, class PointRef>
00125 pointHit triangle<Point, PointRef>::nearestPoint
00126 (
00127 const Point& baseVertex,
00128 const vector& E0,
00129 const vector& E1,
00130 const point& P
00131 )
00132 {
00133
00134 const vector D(baseVertex - P);
00135
00136
00137 const scalar a = E0 & E0;
00138 const scalar b = E0 & E1;
00139 const scalar c = E1 & E1;
00140
00141
00142 const scalar d = E0 & D;
00143 const scalar e = E1 & D;
00144 const scalar f = D & D;
00145
00146
00147 const scalar det = a*c - b*b;
00148 scalar s = b*e - c*d;
00149 scalar t = b*d - a*e;
00150
00151 bool inside = false;
00152
00153 if (s+t < det)
00154 {
00155 if (s < 0)
00156 {
00157 if (t < 0)
00158 {
00159
00160 if (e > 0)
00161 {
00162
00163 t = 0;
00164 s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
00165 }
00166 else
00167 {
00168
00169 s = 0;
00170 t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
00171 }
00172 }
00173 else
00174 {
00175
00176 s = 0;
00177 t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
00178 }
00179 }
00180 else if (t < 0)
00181 {
00182
00183 t = 0;
00184 s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
00185 }
00186 else
00187 {
00188
00189 const scalar invDet = 1/det;
00190 s *= invDet;
00191 t *= invDet;
00192
00193 inside = true;
00194 }
00195 }
00196 else
00197 {
00198 if (s < 0)
00199 {
00200
00201 const scalar tmp0 = b + d;
00202 const scalar tmp1 = c + e;
00203 if (tmp1 > tmp0)
00204 {
00205
00206 const scalar numer = tmp1 - tmp0;
00207 const scalar denom = a-2*b+c;
00208 s = (numer >= denom ? 1 : numer/denom);
00209 t = 1 - s;
00210 }
00211 else
00212 {
00213
00214 s = 0;
00215 t = (tmp1 <= 0 ? 1 : (e >= 0 ? 0 : - e/c));
00216 }
00217 }
00218 else if (t < 0)
00219 {
00220
00221 const scalar tmp0 = b + d;
00222 const scalar tmp1 = c + e;
00223 if (tmp1 > tmp0)
00224 {
00225
00226 const scalar numer = tmp1 - tmp0;
00227 const scalar denom = a-2*b+c;
00228 s = (numer >= denom ? 1 : numer/denom);
00229 t = 1 - s;
00230 }
00231 else
00232 {
00233
00234 t = 0;
00235 s = (tmp1 <= 0 ? 1 : (d >= 0 ? 0 : - d/a));
00236 }
00237 }
00238 else
00239 {
00240
00241 const scalar numer = c+e-(b+d);
00242 if (numer <= 0)
00243 {
00244 s = 0;
00245 }
00246 else
00247 {
00248 const scalar denom = a-2*b+c;
00249 s = (numer >= denom ? 1 : numer/denom);
00250 }
00251 }
00252
00253 t = 1 - s;
00254 }
00255
00256
00257
00258
00259
00260
00261 return pointHit
00262 (
00263 inside,
00264 baseVertex + s*E0 + t*E1,
00265 Foam::sqrt
00266 (
00267 Foam::mag(a*s*s + 2*b*s*t + c*t*t + 2*d*s + 2*e*t + f)
00268 ),
00269 !inside
00270 );
00271 }
00272
00273
00274
00275
00276 template<class Point, class PointRef>
00277 inline triangle<Point, PointRef>::triangle
00278 (
00279 const Point& a,
00280 const Point& b,
00281 const Point& c
00282 )
00283 :
00284 a_(a),
00285 b_(b),
00286 c_(c)
00287 {}
00288
00289
00290 template<class Point, class PointRef>
00291 inline triangle<Point, PointRef>::triangle(Istream& is)
00292 {
00293
00294 is.readBegin("triangle");
00295
00296 is >> a_ >> b_ >> c_;
00297
00298
00299 is.readEnd("triangle");
00300
00301
00302 is.check("triangle::triangle(Istream& is)");
00303 }
00304
00305
00306
00307
00308 template<class Point, class PointRef>
00309 inline const Point& triangle<Point, PointRef>::a() const
00310 {
00311 return a_;
00312 }
00313
00314 template<class Point, class PointRef>
00315 inline const Point& triangle<Point, PointRef>::b() const
00316 {
00317 return b_;
00318 }
00319
00320 template<class Point, class PointRef>
00321 inline const Point& triangle<Point, PointRef>::c() const
00322 {
00323 return c_;
00324 }
00325
00326
00327 template<class Point, class PointRef>
00328 inline Point triangle<Point, PointRef>::centre() const
00329 {
00330 return 1.0/3.0*(a_ + b_ + c_);
00331 }
00332
00333
00334 template<class Point, class PointRef>
00335 inline scalar triangle<Point, PointRef>::mag() const
00336 {
00337 return ::Foam::mag(normal());
00338 }
00339
00340
00341 template<class Point, class PointRef>
00342 inline vector triangle<Point, PointRef>::normal() const
00343 {
00344 return 0.5*((b_ - a_)^(c_ - a_));
00345 }
00346
00347
00348 template<class Point, class PointRef>
00349 inline vector triangle<Point, PointRef>::circumCentre() const
00350 {
00351 scalar d1 = (c_ - a_)&(b_ - a_);
00352 scalar d2 = -(c_ - b_)&(b_ - a_);
00353 scalar d3 = (c_ - a_)&(c_ - b_);
00354
00355 scalar c1 = d2*d3;
00356 scalar c2 = d3*d1;
00357 scalar c3 = d1*d2;
00358
00359 scalar c = c1 + c2 + c3;
00360
00361 return
00362 (
00363 ((c2 + c3)*a_ + (c3 + c1)*b_ + (c1 + c2)*c_)/(2*c)
00364 );
00365 }
00366
00367
00368 template<class Point, class PointRef>
00369 inline scalar triangle<Point, PointRef>::circumRadius() const
00370 {
00371 scalar d1 = (c_ - a_) & (b_ - a_);
00372 scalar d2 = - (c_ - b_) & (b_ - a_);
00373 scalar d3 = (c_ - a_) & (c_ - b_);
00374
00375 scalar denom = d2*d3 + d3*d1 + d1*d2;
00376
00377 if (Foam::mag(denom) < VSMALL)
00378 {
00379 return GREAT;
00380 }
00381 else
00382 {
00383 scalar a = (d1 + d2)*(d2 + d3)*(d3 + d1) / denom;
00384
00385 return 0.5*Foam::sqrt(min(GREAT, max(0, a)));
00386 }
00387 }
00388
00389
00390 template<class Point, class PointRef>
00391 inline scalar triangle<Point, PointRef>::quality() const
00392 {
00393 return
00394 mag()
00395 / (
00396 mathematicalConstant::pi
00397 * Foam::sqr(circumRadius())
00398 * 0.413497
00399 + VSMALL
00400 );
00401 }
00402
00403
00404 template<class Point, class PointRef>
00405 inline scalar triangle<Point, PointRef>::sweptVol(const triangle& t) const
00406 {
00407 return (1.0/6.0)*
00408 (
00409 ((t.a_ - a_) & ((b_ - a_)^(c_ - a_)))
00410 + ((t.b_ - b_) & ((c_ - b_)^(t.a_ - b_)))
00411 + ((c_ - t.c_) & ((t.b_ - t.c_)^(t.a_ - t.c_)))
00412 );
00413 }
00414
00415
00416 template<class Point, class PointRef>
00417 inline pointHit triangle<Point, PointRef>::ray
00418 (
00419 const point& p,
00420 const vector& q,
00421 const intersection::algorithm alg,
00422 const intersection::direction dir
00423 ) const
00424 {
00425
00426
00427 vector E0 = b_ - a_;
00428 vector E1 = c_ - a_;
00429
00430
00431 pointHit inter(p);
00432
00433 vector n(0.5*(E0 ^ E1));
00434 scalar area = Foam::mag(n);
00435
00436 if (area < VSMALL)
00437 {
00438
00439 inter.setMiss(false);
00440
00441
00442 inter.setPoint(a_);
00443
00444
00445
00446
00447 inter.setDistance(Foam::mag(a_ - p));
00448
00449 return inter;
00450 }
00451
00452 n /= area;
00453
00454 vector q1 = q/Foam::mag(q);
00455
00456 if (dir == intersection::CONTACT_SPHERE)
00457 {
00458 return ray(p, q1 - n, alg, intersection::VECTOR);
00459 }
00460
00461 point pInter;
00462
00463
00464 bool hit = intersection(a_, E0, E1, n, p, q1, pInter);
00465
00466 scalar dist = q1 & (pInter - p);
00467
00468 const scalar planarPointTol =
00469 Foam::min
00470 (
00471 Foam::min
00472 (
00473 Foam::mag(E0),
00474 Foam::mag(E1)
00475 ),
00476 Foam::mag(c_ - b_)
00477 )*intersection::planarTol();
00478
00479 bool eligible =
00480 alg == intersection::FULL_RAY
00481 || alg == intersection::HALF_RAY && dist > -planarPointTol
00482 || (
00483 alg == intersection::VISIBLE
00484 && ((q1 & normal()) < -VSMALL)
00485 );
00486
00487 if (hit && eligible)
00488 {
00489
00490 inter.setHit();
00491 inter.setPoint(pInter);
00492 inter.setDistance(dist);
00493 }
00494 else
00495 {
00496
00497 inter.setMiss(eligible);
00498
00499
00500 inter.setPoint(nearestPoint(a_, E0, E1, p).rawPoint());
00501
00502
00503
00504 inter.setDistance(Foam::mag(pInter - p));
00505 }
00506
00507 return inter;
00508 }
00509
00510
00511
00512 template<class Point, class PointRef>
00513 inline pointHit triangle<Point, PointRef>::nearestPoint
00514 (
00515 const point& p
00516 ) const
00517 {
00518
00519
00520 vector E0 = b_ - a_;
00521 vector E1 = c_ - a_;
00522
00523 return nearestPoint(a_, E0, E1, p);
00524 }
00525
00526
00527 template<class Point, class PointRef>
00528 inline bool triangle<Point, PointRef>::classify
00529 (
00530 const point& p,
00531 const scalar tol,
00532 label& nearType,
00533 label& nearLabel
00534 ) const
00535 {
00536 const vector E0 = b_ - a_;
00537 const vector E1 = c_ - a_;
00538 const vector n = 0.5*(E0 ^ E1);
00539
00540
00541 scalar magX = Foam::mag(n.x());
00542 scalar magY = Foam::mag(n.y());
00543 scalar magZ = Foam::mag(n.z());
00544
00545 label i0 = -1;
00546 if ((magX >= magY) && (magX >= magZ))
00547 {
00548 i0 = 0;
00549 }
00550 else if ((magY >= magX) && (magY >= magZ))
00551 {
00552 i0 = 1;
00553 }
00554 else
00555 {
00556 i0 = 2;
00557 }
00558
00559
00560 label i1 = (i0 + 1) % 3;
00561 label i2 = (i1 + 1) % 3;
00562
00563
00564 scalar u1 = E0[i1];
00565 scalar v1 = E0[i2];
00566
00567 scalar u2 = E1[i1];
00568 scalar v2 = E1[i2];
00569
00570 scalar det = v2*u1 - u2*v1;
00571
00572 scalar u0 = p[i1] - a_[i1];
00573 scalar v0 = p[i2] - a_[i2];
00574
00575 scalar alpha = 0;
00576 scalar beta = 0;
00577
00578 bool hit = false;
00579
00580 if (Foam::mag(u1) < SMALL)
00581 {
00582 beta = u0/u2;
00583
00584 alpha = (v0 - beta*v2)/v1;
00585
00586 hit = ((alpha >= 0) && ((alpha + beta) <= 1));
00587 }
00588 else
00589 {
00590 beta = (v0*u1 - u0*v1)/det;
00591
00592 alpha = (u0 - beta*u2)/u1;
00593
00594 hit = ((alpha >= 0) && ((alpha + beta) <= 1));
00595 }
00596
00597
00598
00599
00600
00601
00602 nearType = NONE;
00603 nearLabel = -1;
00604
00605 if (Foam::mag(alpha+beta-1) < tol)
00606 {
00607
00608
00609 if (Foam::mag(alpha) < tol)
00610 {
00611 nearType = POINT;
00612 nearLabel = 2;
00613 }
00614 else if (Foam::mag(beta) < tol)
00615 {
00616 nearType = POINT;
00617 nearLabel = 1;
00618 }
00619 else if ((alpha >= 0) && (alpha <= 1) && (beta >= 0) && (beta <= 1))
00620 {
00621 nearType = EDGE;
00622 nearLabel = 1;
00623 }
00624 }
00625 else if (Foam::mag(alpha) < tol)
00626 {
00627
00628
00629 if (Foam::mag(beta) < tol)
00630 {
00631 nearType = POINT;
00632 nearLabel = 0;
00633 }
00634 else if (Foam::mag(beta-1) < tol)
00635 {
00636 nearType = POINT;
00637 nearLabel = 2;
00638 }
00639 else if ((beta >= 0) && (beta <= 1))
00640 {
00641 nearType = EDGE;
00642 nearLabel = 2;
00643 }
00644 }
00645 else if (Foam::mag(beta) < tol)
00646 {
00647
00648
00649 if (Foam::mag(alpha) < tol)
00650 {
00651 nearType = POINT;
00652 nearLabel = 0;
00653 }
00654 else if (Foam::mag(alpha-1) < tol)
00655 {
00656 nearType = POINT;
00657 nearLabel = 1;
00658 }
00659 else if ((alpha >= 0) && (alpha <= 1))
00660 {
00661 nearType = EDGE;
00662 nearLabel = 0;
00663 }
00664 }
00665
00666 return hit;
00667 }
00668
00669
00670
00671
00672
00673
00674
00675 template<class point, class pointRef>
00676 inline Istream& operator>>(Istream& is, triangle<point, pointRef>& t)
00677 {
00678
00679 is.readBegin("triangle");
00680
00681 is >> t.a_ >> t.b_ >> t.c_;
00682
00683
00684 is.readEnd("triangle");
00685
00686
00687 is.check("Istream& operator>>(Istream&, triangle&)");
00688
00689 return is;
00690 }
00691
00692
00693 template<class Point, class PointRef>
00694 inline Ostream& operator<<(Ostream& os, const triangle<Point, PointRef>& t)
00695 {
00696 os << nl
00697 << token::BEGIN_LIST
00698 << t.a_ << token::SPACE << t.b_ << token::SPACE << t.c_
00699 << token::END_LIST;
00700
00701 return os;
00702 }
00703
00704
00705
00706
00707 }
00708
00709
00710