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
 457 {
 458     const vector edge1 = b_ - a_;
 459     const vector edge2 = c_ - a_;
 460
 461     // begin calculating determinant - also used to calculate U parameter
 462     const vector pVec = dir ^ edge2;
 463
 464     // if determinant is near zero, ray lies in plane of triangle
 465     const scalar det = edge1 & pVec;
 466
 467     // Initialise to miss
 468     pointHit intersection(false, vector::zero, GREAT, false);
 469
 470     if (alg == intersection::VISIBLE)
 471     {
 472         // Culling branch
 473         if (det < SMALL)
 474         {
 475             // return miss
 476             return intersection;
 477         }
 478         /* calculate distance from a_ to ray origin */
 479         const vector tVec = orig-a_;
 480
 481         /* calculate U parameter and test bounds */
 482         scalar u = tVec & pVec;
 483
 484         if (u < 0.0 || u > det)
 485         {
 486             // return miss
 487             return intersection;
 488         }
 489
 490         /* prepare to test V parameter */
 491         const vector qVec = tVec ^ edge1;
 492
 493         /* calculate V parameter and test bounds */
 494         scalar v = dir & qVec;
 495
 496         if (v < 0.0 || u + v > det)
 497         {
 498             // return miss
 499             return intersection;
 500         }
 501
 502         /* calculate t, scale parameters, ray intersects triangle */
 503         scalar t = edge2 & qVec;
 504         scalar inv_det = 1.0 / det;
 505         t *= inv_det;
 506         u *= inv_det;
 507         v *= inv_det;
 508
 509         intersection.setHit();
 510         intersection.setPoint(a_ + u*edge1 + v*edge2);
 511         intersection.setDistance(t);
 512     }
 513     else if (alg == intersection::HALF_RAY || alg == intersection::FULL_RAY)
 514     {
 515         // Non-culling branch
 516         if (det > -SMALL && det < SMALL)
 517         {
 518             // return miss
 519             return intersection;
 520         }
 521         const scalar inv_det = 1.0 / det;
 522
 523         /* calculate distance from a_ to ray origin */
 524         const vector tVec = orig - a_;
 525         /* calculate U parameter and test bounds */
 526         const scalar u = (tVec & pVec)*inv_det;
 527
 528         if (u < 0.0 || u > 1.0)
 529         {
 530             // return miss
 531             return intersection;
 532         }
 533         /* prepare to test V parameter */
 534         const vector qVec = tVec ^ edge1;
 535         /* calculate V parameter and test bounds */
 536         const scalar v = (dir & qVec) * inv_det;
 537
 538         if (v < 0.0 || u + v > 1.0)
 539         {
 540             // return miss
 541             return intersection;
 542         }
 543         /* calculate t, ray intersects triangle */
 544         const scalar t = (edge2 & qVec) * inv_det;
 545
 546         if (alg == intersection::HALF_RAY && t < 0)
 547         {
 548             // return miss
 549             return intersection;
 550         }
 551
 552         intersection.setHit();
 553         intersection.setPoint(a_ + u*edge1 + v*edge2);
 554         intersection.setDistance(t);
 555     }
 556     else
 557     {
 558         FatalErrorIn
 559         (
 560             "triangle<Point, PointRef>::intersection(const point&"
 561             ", const vector&, const intersection::algorithm)"
 562         )   << "intersection only defined for VISIBLE, FULL_RAY or HALF_RAY"
 563             << abort(FatalError);
 564     }
 565
 566     return intersection;
 567 }
 568
 569
 570 template<class Point, class PointRef>
 571 inline pointHit triangle<Point, PointRef>::nearestPoint
 572 (
 573     const point& p
 574 ) const
 575 {
 576     // Express triangle in terms of baseVertex (point a_) and
 577     // two edge vectors
 578     vector E0 = b_ - a_;
 579     vector E1 = c_ - a_;
 580
 581     return nearestPoint(a_, E0, E1, p);
 582 }
 583
 584
 585 template<class Point, class PointRef>
 586 inline bool triangle<Point, PointRef>::classify
 587 (
 588     const point& p,
 589     const scalar tol,
 590     label& nearType,
 591     label& nearLabel
 592 ) const
 593 {
 594     const vector E0 = b_ - a_;
 595     const vector E1 = c_ - a_;
 596     const vector n = 0.5*(E0 ^ E1);
 597
 598     // Get largest component of normal
 599     scalar magX = Foam::mag(n.x());
 600     scalar magY = Foam::mag(n.y());
 601     scalar magZ = Foam::mag(n.z());
 602
 603     label i0 = -1;
 604     if ((magX >= magY) && (magX >= magZ))
 605     {
 606         i0 = 0;
 607     }
 608     else if ((magY >= magX) && (magY >= magZ))
 609     {
 610         i0 = 1;
 611     }
 612     else
 613     {
 614         i0 = 2;
 615     }
 616
 617     // Get other components
 618     label i1 = (i0 + 1) % 3;
 619     label i2 = (i1 + 1) % 3;
 620
 621
 622     scalar u1 = E0[i1];
 623     scalar v1 = E0[i2];
 624
 625     scalar u2 = E1[i1];
 626     scalar v2 = E1[i2];
 627
 628     scalar det = v2*u1 - u2*v1;
 629
 630     scalar u0 = p[i1] - a_[i1];
 631     scalar v0 = p[i2] - a_[i2];
 632
 633     scalar alpha = 0;
 634     scalar beta = 0;
 635
 636     bool hit = false;
 637
 638     if (Foam::mag(u1) < SMALL)
 639     {
 640         beta = u0/u2;
 641
 642         alpha = (v0 - beta*v2)/v1;
 643
 644         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 645     }
 646     else
 647     {
 648         beta = (v0*u1 - u0*v1)/det;
 649
 650         alpha = (u0 - beta*u2)/u1;
 651
 652         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 653     }
 654
 655     //
 656     // Now alpha, beta are the coordinates in the triangle local coordinate
 657     // system E0, E1
 658     //
 659
 660     nearType = NONE;
 661     nearLabel = -1;
 662
 663     if (Foam::mag(alpha+beta-1) <= tol)
 664     {
 665         // On edge between vert 1-2 (=edge 1)
 666
 667         if (Foam::mag(alpha) <= tol)
 668         {
 669             nearType = POINT;
 670             nearLabel = 2;
 671         }
 672         else if (Foam::mag(beta) <= tol)
 673         {
 674             nearType = POINT;
 675             nearLabel = 1;
 676         }
 677         else if ((alpha >= 0) && (alpha <= 1) && (beta >= 0) && (beta <= 1))
 678         {
 679             nearType = EDGE;
 680             nearLabel = 1;
 681         }
 682     }
 683     else if (Foam::mag(alpha) <= tol)
 684     {
 685         // On edge between vert 2-0 (=edge 2)
 686
 687         if (Foam::mag(beta) <= tol)
 688         {
 689             nearType = POINT;
 690             nearLabel = 0;
 691         }
 692         else if (Foam::mag(beta-1) <= tol)
 693         {
 694             nearType = POINT;
 695             nearLabel = 2;
 696         }
 697         else if ((beta >= 0) && (beta <= 1))
 698         {
 699             nearType = EDGE;
 700             nearLabel = 2;
 701         }
 702     }
 703     else if (Foam::mag(beta) <= tol)
 704     {
 705         // On edge between vert 0-1 (= edge 0)
 706
 707         if (Foam::mag(alpha) <= tol)
 708         {
 709             nearType = POINT;
 710             nearLabel = 0;
 711         }
 712         else if (Foam::mag(alpha-1) <= tol)
 713         {
 714             nearType = POINT;
 715             nearLabel = 1;
 716         }
 717         else if ((alpha >= 0) && (alpha <= 1))
 718         {
 719             nearType = EDGE;
 720             nearLabel = 0;
 721         }
 722     }
 723
 724     return hit;
 725 }
 726
 727
 728
 729
 730
 731 // * * * * * * * * * * * * * * * Ostream Operator  * * * * * * * * * * * * * //
 732
 733 template<class point, class pointRef>
 734 inline Istream& operator>>(Istream& is, triangle<point, pointRef>& t)
 735 {
 736     // Read beginning of triangle point pair
 737     is.readBegin("triangle");
 738
 739     is >> t.a_ >> t.b_ >> t.c_;
 740
 741     // Read end of triangle point pair
 742     is.readEnd("triangle");
 743
 744     // Check state of Ostream
 745     is.check("Istream& operator>>(Istream&, triangle&)");
 746
 747     return is;
 748 }
 749
 750
 751 template<class Point, class PointRef>
 752 inline Ostream& operator<<(Ostream& os, const triangle<Point, PointRef>& t)
 753 {
 754     os  << nl
 755         << token::BEGIN_LIST
 756         << t.a_ << token::SPACE << t.b_ << token::SPACE << t.c_
 757         << token::END_LIST;
 758
 759     return os;
 760 }
 761
 762
 763 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 764
 765 } // End namespace Foam
 766
 767 // ************************************************************************* //
 768