src/OpenFOAM/meshes/primitiveShapes/triangle/triangleI.H

   1 /*---------------------------------------------------------------------------*\
   2   =========                 |
   3   \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   4    \\    /   O peration     |
   5     \\  /    A nd           | Copyright (C) 1991-2009 OpenCFD Ltd.
   6      \\/     M anipulation  |
   7 -------------------------------------------------------------------------------
   8 License
   9     This file is part of OpenFOAM.
  10
  11     OpenFOAM is free software; you can redistribute it and/or modify it
  12     under the terms of the GNU General Public License as published by the
  13     Free Software Foundation; either version 2 of the License, or (at your
  14     option) any later version.
  15
  16     OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
  17     ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  18     FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  19     for more details.
  20
  21     You should have received a copy of the GNU General Public License
  22     along with OpenFOAM; if not, write to the Free Software Foundation,
  23     Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  24
  25 \*---------------------------------------------------------------------------*/
  26
  27 #include "IOstreams.H"
  28 #include "pointHit.H"
  29 #include "mathematicalConstants.H"
  30
  31 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
  32
  33 namespace Foam
  34 {
  35
  36 // * * * * * * * * * * * * * Private Member Functions  * * * * * * * * * * * //
  37
  38 template<class Point, class PointRef>
  39 pointHit triangle<Point, PointRef>::nearestPoint
  40 (
  41     const Point& baseVertex,
  42     const vector& E0,
  43     const vector& E1,
  44     const point& P
  45 )
  46 {
  47     // Distance vector
  48     const vector D(baseVertex - P);
  49
  50     // Some geometrical factors
  51     const scalar a = E0 & E0;
  52     const scalar b = E0 & E1;
  53     const scalar c = E1 & E1;
  54
  55     // Precalculate distance factors
  56     const scalar d = E0 & D;
  57     const scalar e = E1 & D;
  58     const scalar f = D & D;
  59
  60     // Do classification
  61     const scalar det = a*c - b*b;
  62     scalar s = b*e - c*d;
  63     scalar t = b*d - a*e;
  64
  65     bool inside = false;
  66
  67     if (s+t < det)
  68     {
  69         if (s < 0)
  70         {
  71             if (t < 0)
  72             {
  73                 // Region 4
  74                 if (e > 0)
  75                 {
  76                     // min on edge t = 0
  77                     t = 0;
  78                     s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
  79                 }
  80                 else
  81                 {
  82                     // min on edge s=0
  83                     s = 0;
  84                     t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
  85                 }
  86             }
  87             else
  88             {
  89                 // Region 3. Min on edge s = 0
  90                 s = 0;
  91                 t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
  92             }
  93         }
  94         else if (t < 0)
  95         {
  96             // Region 5
  97             t = 0;
  98             s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
  99         }
 100         else
 101         {
 102             // Region 0
 103             const scalar invDet = 1/det;
 104             s *= invDet;
 105             t *= invDet;
 106
 107             inside = true;
 108         }
 109     }
 110     else
 111     {
 112         if (s < 0)
 113         {
 114             // Region 2
 115             const scalar tmp0 = b + d;
 116             const scalar tmp1 = c + e;
 117             if (tmp1 > tmp0)
 118             {
 119                 // min on edge s+t=1
 120                 const scalar numer = tmp1 - tmp0;
 121                 const scalar denom = a-2*b+c;
 122                 s = (numer >= denom ? 1 : numer/denom);
 123                 t = 1 - s;
 124             }
 125             else
 126             {
 127                 // min on edge s=0
 128                 s = 0;
 129                 t = (tmp1 <= 0 ? 1 : (e >= 0 ? 0 : - e/c));
 130             }
 131         }
 132         else if (t < 0)
 133         {
 134             // Region 6
 135             const scalar tmp0 = b + d;
 136             const scalar tmp1 = c + e;
 137             if (tmp1 > tmp0)
 138             {
 139                 // min on edge s+t=1
 140                 const scalar numer = tmp1 - tmp0;
 141                 const scalar denom = a-2*b+c;
 142                 s = (numer >= denom ? 1 : numer/denom);
 143                 t = 1 - s;
 144             }
 145             else
 146             {
 147                 // min on edge t=0
 148                 t = 0;
 149                 s = (tmp1 <= 0 ? 1 : (d >= 0 ? 0 : - d/a));
 150             }
 151         }
 152         else
 153         {
 154             // Region 1
 155             const scalar numer = c+e-(b+d);
 156             if (numer <= 0)
 157             {
 158                 s = 0;
 159             }
 160             else
 161             {
 162                 const scalar denom = a-2*b+c;
 163                 s = (numer >= denom ? 1 : numer/denom);
 164             }
 165         }
 166
 167         t = 1 - s;
 168     }
 169
 170     // Calculate distance.
 171     // Note: Foam::mag used since truncation error causes negative distances
 172     // with points very close to one of the triangle vertices.
 173     // (Up to -2.77556e-14 seen). Could use +SMALL but that not large enough.
 174
 175     return pointHit
 176     (
 177         inside,
 178         baseVertex + s*E0 + t*E1,
 179         Foam::sqrt
 180         (
 181             Foam::mag(a*s*s + 2*b*s*t + c*t*t + 2*d*s + 2*e*t + f)
 182         ),
 183         !inside
 184     );
 185 }
 186
 187
 188 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
 189
 190 template<class Point, class PointRef>
 191 inline triangle<Point, PointRef>::triangle
 192 (
 193     const Point& a,
 194     const Point& b,
 195     const Point& c
 196 )
 197 :
 198     a_(a),
 199     b_(b),
 200     c_(c)
 201 {}
 202
 203
 204 template<class Point, class PointRef>
 205 inline triangle<Point, PointRef>::triangle(Istream& is)
 206 {
 207     // Read beginning of triangle point pair
 208     is.readBegin("triangle");
 209
 210     is >> a_ >> b_ >> c_;
 211
 212     // Read end of triangle point pair
 213     is.readEnd("triangle");
 214
 215     // Check state of Istream
 216     is.check("triangle::triangle(Istream& is)");
 217 }
 218
 219
 220 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 221
 222 template<class Point, class PointRef>
 223 inline const Point& triangle<Point, PointRef>::a() const
 224 {
 225     return a_;
 226 }
 227
 228 template<class Point, class PointRef>
 229 inline const Point& triangle<Point, PointRef>::b() const
 230 {
 231     return b_;
 232 }
 233
 234 template<class Point, class PointRef>
 235 inline const Point& triangle<Point, PointRef>::c() const
 236 {
 237     return c_;
 238 }
 239
 240
 241 template<class Point, class PointRef>
 242 inline Point triangle<Point, PointRef>::centre() const
 243 {
 244     return (1.0/3.0)*(a_ + b_ + c_);
 245 }
 246
 247
 248 template<class Point, class PointRef>
 249 inline scalar triangle<Point, PointRef>::mag() const
 250 {
 251     return ::Foam::mag(normal());
 252 }
 253
 254
 255 template<class Point, class PointRef>
 256 inline vector triangle<Point, PointRef>::normal() const
 257 {
 258     return 0.5*((b_ - a_)^(c_ - a_));
 259 }
 260
 261
 262 template<class Point, class PointRef>
 263 inline vector triangle<Point, PointRef>::circumCentre() const
 264 {
 265     scalar d1 = (c_ - a_)&(b_ - a_);
 266     scalar d2 = -(c_ - b_)&(b_ - a_);
 267     scalar d3 = (c_ - a_)&(c_ - b_);
 268
 269     scalar c1 = d2*d3;
 270     scalar c2 = d3*d1;
 271     scalar c3 = d1*d2;
 272
 273     scalar c = c1 + c2 + c3;
 274
 275     return
 276     (
 277         ((c2 + c3)*a_ + (c3 + c1)*b_ + (c1 + c2)*c_)/(2*c)
 278     );
 279 }
 280
 281
 282 template<class Point, class PointRef>
 283 inline scalar triangle<Point, PointRef>::circumRadius() const
 284 {
 285     scalar d1 = (c_ - a_) & (b_ - a_);
 286     scalar d2 = - (c_ - b_) & (b_ - a_);
 287     scalar d3 = (c_ - a_) & (c_ - b_);
 288
 289     scalar denom = d2*d3 + d3*d1 + d1*d2;
 290
 291     if (Foam::mag(denom) < VSMALL)
 292     {
 293         return GREAT;
 294     }
 295     else
 296     {
 297         scalar a = (d1 + d2)*(d2 + d3)*(d3 + d1) / denom;
 298
 299         return 0.5*Foam::sqrt(min(GREAT, max(0, a)));
 300     }
 301 }
 302
 303
 304 template<class Point, class PointRef>
 305 inline scalar triangle<Point, PointRef>::quality() const
 306 {
 307     return
 308         mag()
 309       / (
 310             mathematicalConstant::pi
 311           * Foam::sqr(circumRadius())
 312           * 0.413497
 313           + VSMALL
 314         );
 315 }
 316
 317
 318 template<class Point, class PointRef>
 319 inline scalar triangle<Point, PointRef>::sweptVol(const triangle& t) const
 320 {
 321     return (1.0/12.0)*
 322     (
 323         ((t.a_ - a_) & ((b_ - a_)^(c_ - a_)))
 324       + ((t.b_ - b_) & ((c_ - b_)^(t.a_ - b_)))
 325       + ((c_ - t.c_) & ((t.b_ - t.c_)^(t.a_ - t.c_)))
 326
 327       + ((t.a_ - a_) & ((b_ - a_)^(c_ - a_)))
 328       + ((b_ - t.b_) & ((t.a_ - t.b_)^(t.c_ - t.b_)))
 329       + ((c_ - t.c_) & ((b_ - t.c_)^(t.a_ - t.c_)))
 330     );
 331 }
 332
 333
 334 template<class Point, class PointRef>
 335 inline pointHit triangle<Point, PointRef>::ray
 336 (
 337     const point& p,
 338     const vector& q,
 339     const intersection::algorithm alg,
 340     const intersection::direction dir
 341 ) const
 342 {
 343     // Express triangle in terms of baseVertex (point a_) and
 344     // two edge vectors
 345     vector E0 = b_ - a_;
 346     vector E1 = c_ - a_;
 347
 348     // Initialize intersection to miss.
 349     pointHit inter(p);
 350
 351     vector n(0.5*(E0 ^ E1));
 352     scalar area = Foam::mag(n);
 353
 354     if (area < VSMALL)
 355     {
 356         // Ineligible miss.
 357         inter.setMiss(false);
 358
 359         // The miss point is the nearest point on the triangle. Take any one.
 360         inter.setPoint(a_);
 361
 362         // The distance to the miss is the distance between the
 363         // original point and plane of intersection. No normal so use
 364         // distance from p to a_
 365         inter.setDistance(Foam::mag(a_ - p));
 366
 367         return inter;
 368     }
 369
 370     vector q1 = q/Foam::mag(q);
 371
 372     if (dir == intersection::CONTACT_SPHERE)
 373     {
 374         n /= area;
 375
 376         return ray(p, q1 - n, alg, intersection::VECTOR);
 377     }
 378
 379     // Intersection point with triangle plane
 380     point pInter;
 381     // Is intersection point inside triangle
 382     bool hit;
 383     {
 384         // Reuse the fast ray intersection routine below in FULL_RAY
 385         // mode since the original intersection routine has rounding problems.
 386         pointHit fastInter = intersection(p, q1, intersection::FULL_RAY);
 387         hit = fastInter.hit();
 388
 389         if (hit)
 390         {
 391             pInter = fastInter.rawPoint();
 392         }
 393         else
 394         {
 395             // Calculate intersection of ray with triangle plane
 396             vector v = a_ - p;
 397             pInter = p + (q1&v)*q1;
 398         }
 399     }
 400
 401     // Distance to intersection point
 402     scalar dist = q1 & (pInter - p);
 403
 404     const scalar planarPointTol =
 405         Foam::min
 406         (
 407             Foam::min
 408             (
 409                 Foam::mag(E0),
 410                 Foam::mag(E1)
 411             ),
 412             Foam::mag(c_ - b_)
 413         )*intersection::planarTol();
 414
 415     bool eligible =
 416         alg == intersection::FULL_RAY
 417      || (alg == intersection::HALF_RAY && dist > -planarPointTol)
 418      || (
 419             alg == intersection::VISIBLE
 420          && ((q1 & normal()) < -VSMALL)
 421         );
 422
 423     if (hit && eligible)
 424     {
 425         // Hit. Set distance to intersection.
 426         inter.setHit();
 427         inter.setPoint(pInter);
 428         inter.setDistance(dist);
 429     }
 430     else
 431     {
 432         // Miss or ineligible hit.
 433         inter.setMiss(eligible);
 434
 435         // The miss point is the nearest point on the triangle
 436         inter.setPoint(nearestPoint(a_, E0, E1, p).rawPoint());
 437
 438         // The distance to the miss is the distance between the
 439         // original point and plane of intersection
 440         inter.setDistance(Foam::mag(pInter - p));
 441     }
 442
 443
 444     return inter;
 445 }
 446
 447
 448 // From "Fast, Minimum Storage Ray/Triangle Intersection"
 449 // Moeller/Trumbore.
 450 template<class Point, class PointRef>
 451 inline pointHit triangle<Point, PointRef>::intersection
 452 (
 453     const point& orig,
 454     const vector& dir,
 455     const intersection::algorithm alg,
 456     const scalar tol
 457 ) const
 458 {
 459     const vector edge1 = b_ - a_;
 460     const vector edge2 = c_ - a_;
 461
 462     // begin calculating determinant - also used to calculate U parameter
 463     const vector pVec = dir ^ edge2;
 464
 465     // if determinant is near zero, ray lies in plane of triangle
 466     const scalar det = edge1 & pVec;
 467
 468     // Initialise to miss
 469     pointHit intersection(false, vector::zero, GREAT, false);
 470
 471     if (alg == intersection::VISIBLE)
 472     {
 473         // Culling branch
 474         if (det < ROOTVSMALL)
 475         {
 476             // Ray on wrong side of triangle. Return miss
 477             return intersection;
 478         }
 479     }
 480     else if (alg == intersection::HALF_RAY || alg == intersection::FULL_RAY)
 481     {
 482         // Non-culling branch
 483         if (det > -ROOTVSMALL && det < ROOTVSMALL)
 484         {
 485             // Ray parallel to triangle. Return miss
 486             return intersection;
 487         }
 488     }
 489
 490     const scalar inv_det = 1.0 / det;
 491
 492     /* calculate distance from a_ to ray origin */
 493     const vector tVec = orig-a_;
 494
 495     /* calculate U parameter and test bounds */
 496     const scalar u = (tVec & pVec)*inv_det;
 497
 498     if (u < -tol || u > 1.0+tol)
 499     {
 500         // return miss
 501         return intersection;
 502     }
 503
 504     /* prepare to test V parameter */
 505     const vector qVec = tVec ^ edge1;
 506
 507     /* calculate V parameter and test bounds */
 508     const scalar v = (dir & qVec) * inv_det;
 509
 510     if (v < -tol || u + v > 1.0+tol)
 511     {
 512         // return miss
 513         return intersection;
 514     }
 515
 516     /* calculate t, scale parameters, ray intersects triangle */
 517     const scalar t = (edge2 & qVec) * inv_det;
 518
 519     if (alg == intersection::HALF_RAY && t < -tol)
 520     {
 521         // Wrong side of orig. Return miss
 522         return intersection;
 523     }
 524
 525     intersection.setHit();
 526     intersection.setPoint(a_ + u*edge1 + v*edge2);
 527     intersection.setDistance(t);
 528
 529     return intersection;
 530 }
 531
 532
 533 template<class Point, class PointRef>
 534 inline pointHit triangle<Point, PointRef>::nearestPoint
 535 (
 536     const point& p
 537 ) const
 538 {
 539     // Express triangle in terms of baseVertex (point a_) and
 540     // two edge vectors
 541     vector E0 = b_ - a_;
 542     vector E1 = c_ - a_;
 543
 544     return nearestPoint(a_, E0, E1, p);
 545 }
 546
 547
 548 template<class Point, class PointRef>
 549 inline bool triangle<Point, PointRef>::classify
 550 (
 551     const point& p,
 552     const scalar tol,
 553     label& nearType,
 554     label& nearLabel
 555 ) const
 556 {
 557     const vector E0 = b_ - a_;
 558     const vector E1 = c_ - a_;
 559     const vector n = 0.5*(E0 ^ E1);
 560
 561     // Get largest component of normal
 562     scalar magX = Foam::mag(n.x());
 563     scalar magY = Foam::mag(n.y());
 564     scalar magZ = Foam::mag(n.z());
 565
 566     label i0 = -1;
 567     if ((magX >= magY) && (magX >= magZ))
 568     {
 569         i0 = 0;
 570     }
 571     else if ((magY >= magX) && (magY >= magZ))
 572     {
 573         i0 = 1;
 574     }
 575     else
 576     {
 577         i0 = 2;
 578     }
 579
 580     // Get other components
 581     label i1 = (i0 + 1) % 3;
 582     label i2 = (i1 + 1) % 3;
 583
 584
 585     scalar u1 = E0[i1];
 586     scalar v1 = E0[i2];
 587
 588     scalar u2 = E1[i1];
 589     scalar v2 = E1[i2];
 590
 591     scalar det = v2*u1 - u2*v1;
 592
 593     scalar u0 = p[i1] - a_[i1];
 594     scalar v0 = p[i2] - a_[i2];
 595
 596     scalar alpha = 0;
 597     scalar beta = 0;
 598
 599     bool hit = false;
 600
 601     if (Foam::mag(u1) < ROOTVSMALL)
 602     {
 603         beta = u0/u2;
 604
 605         alpha = (v0 - beta*v2)/v1;
 606
 607         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 608     }
 609     else
 610     {
 611         beta = (v0*u1 - u0*v1)/det;
 612
 613         alpha = (u0 - beta*u2)/u1;
 614
 615         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 616     }
 617
 618     //
 619     // Now alpha, beta are the coordinates in the triangle local coordinate
 620     // system E0, E1
 621     //
 622
 623     //Pout<< "alpha:" << alpha << endl;
 624     //Pout<< "beta:" << beta << endl;
 625     //Pout<< "hit:" << hit << endl;
 626     //Pout<< "tol:" << tol << endl;
 627
 628     if (hit)
 629     {
 630         // alpha,beta might get negative due to precision errors
 631         alpha = max(0.0, min(1.0, alpha));
 632         beta = max(0.0, min(1.0, beta));
 633     }
 634
 635     nearType = NONE;
 636     nearLabel = -1;
 637
 638     if (Foam::mag(alpha+beta-1) <= tol)
 639     {
 640         // On edge between vert 1-2 (=edge 1)
 641
 642         if (Foam::mag(alpha) <= tol)
 643         {
 644             nearType = POINT;
 645             nearLabel = 2;
 646         }
 647         else if (Foam::mag(beta) <= tol)
 648         {
 649             nearType = POINT;
 650             nearLabel = 1;
 651         }
 652         else if ((alpha >= 0) && (alpha <= 1) && (beta >= 0) && (beta <= 1))
 653         {
 654             nearType = EDGE;
 655             nearLabel = 1;
 656         }
 657     }
 658     else if (Foam::mag(alpha) <= tol)
 659     {
 660         // On edge between vert 2-0 (=edge 2)
 661
 662         if (Foam::mag(beta) <= tol)
 663         {
 664             nearType = POINT;
 665             nearLabel = 0;
 666         }
 667         else if (Foam::mag(beta-1) <= tol)
 668         {
 669             nearType = POINT;
 670             nearLabel = 2;
 671         }
 672         else if ((beta >= 0) && (beta <= 1))
 673         {
 674             nearType = EDGE;
 675             nearLabel = 2;
 676         }
 677     }
 678     else if (Foam::mag(beta) <= tol)
 679     {
 680         // On edge between vert 0-1 (= edge 0)
 681
 682         if (Foam::mag(alpha) <= tol)
 683         {
 684             nearType = POINT;
 685             nearLabel = 0;
 686         }
 687         else if (Foam::mag(alpha-1) <= tol)
 688         {
 689             nearType = POINT;
 690             nearLabel = 1;
 691         }
 692         else if ((alpha >= 0) && (alpha <= 1))
 693         {
 694             nearType = EDGE;
 695             nearLabel = 0;
 696         }
 697     }
 698
 699     return hit;
 700 }
 701
 702
 703
 704
 705
 706 // * * * * * * * * * * * * * * * Ostream Operator  * * * * * * * * * * * * * //
 707
 708 template<class point, class pointRef>
 709 inline Istream& operator>>(Istream& is, triangle<point, pointRef>& t)
 710 {
 711     // Read beginning of triangle point pair
 712     is.readBegin("triangle");
 713
 714     is >> t.a_ >> t.b_ >> t.c_;
 715
 716     // Read end of triangle point pair
 717     is.readEnd("triangle");
 718
 719     // Check state of Ostream
 720     is.check("Istream& operator>>(Istream&, triangle&)");
 721
 722     return is;
 723 }
 724
 725
 726 template<class Point, class PointRef>
 727 inline Ostream& operator<<(Ostream& os, const triangle<Point, PointRef>& t)
 728 {
 729     os  << nl
 730         << token::BEGIN_LIST
 731         << t.a_ << token::SPACE << t.b_ << token::SPACE << t.c_
 732         << token::END_LIST;
 733
 734     return os;
 735 }
 736
 737
 738 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 739
 740 } // End namespace Foam
 741
 742 // ************************************************************************* //
 743