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-2008 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     point pInter;
 380     bool hit;
 381     {
 382         // Reuse the fast ray intersection routine below in FULL_RAY
 383         // mode since the original intersection routine has rounding problems.
 384         pointHit fastInter = intersection(p, q1, intersection::FULL_RAY);
 385         pInter = fastInter.rawPoint();
 386         hit = fastInter.hit();
 387     }
 388
 389     scalar dist = q1 & (pInter - p);
 390
 391     const scalar planarPointTol =
 392         Foam::min
 393         (
 394             Foam::min
 395             (
 396                 Foam::mag(E0),
 397                 Foam::mag(E1)
 398             ),
 399             Foam::mag(c_ - b_)
 400         )*intersection::planarTol();
 401
 402     bool eligible =
 403         alg == intersection::FULL_RAY
 404      || (alg == intersection::HALF_RAY && dist > -planarPointTol)
 405      || (
 406             alg == intersection::VISIBLE
 407          && ((q1 & normal()) < -VSMALL)
 408         );
 409
 410     if (hit && eligible)
 411     {
 412         // Hit. Set distance to intersection.
 413         inter.setHit();
 414         inter.setPoint(pInter);
 415         inter.setDistance(dist);
 416     }
 417     else
 418     {
 419         // Miss or ineligible hit.
 420         inter.setMiss(eligible);
 421
 422         // The miss point is the nearest point on the triangle
 423         inter.setPoint(nearestPoint(a_, E0, E1, p).rawPoint());
 424
 425         // The distance to the miss is the distance between the
 426         // original point and plane of intersection
 427         inter.setDistance(Foam::mag(pInter - p));
 428     }
 429
 430
 431     return inter;
 432 }
 433
 434
 435 // From "Fast, Minimum Storage Ray/Triangle Intersection"
 436 // Moeller/Trumbore.
 437 template<class Point, class PointRef>
 438 inline pointHit triangle<Point, PointRef>::intersection
 439 (
 440     const point& orig,
 441     const vector& dir,
 442     const intersection::algorithm alg
 443 ) const
 444 {
 445     const vector edge1 = b_ - a_;
 446     const vector edge2 = c_ - a_;
 447
 448     // begin calculating determinant - also used to calculate U parameter
 449     const vector pVec = dir ^ edge2;
 450
 451     // if determinant is near zero, ray lies in plane of triangle
 452     const scalar det = edge1 & pVec;
 453
 454     // Initialise to miss
 455     pointHit intersection(false, vector::zero, GREAT, false);
 456
 457     if (alg == intersection::VISIBLE)
 458     {
 459         // Culling branch
 460         if (det < SMALL)
 461         {
 462             // return miss
 463             return intersection;
 464         }
 465         /* calculate distance from a_ to ray origin */
 466         const vector tVec = orig-a_;
 467
 468         /* calculate U parameter and test bounds */
 469         scalar u = tVec & pVec;
 470
 471         if (u < 0.0 || u > det)
 472         {
 473             // return miss
 474             return intersection;
 475         }
 476
 477         /* prepare to test V parameter */
 478         const vector qVec = tVec ^ edge1;
 479
 480         /* calculate V parameter and test bounds */
 481         scalar v = dir & qVec;
 482
 483         if (v < 0.0 || u + v > det)
 484         {
 485             // return miss
 486             return intersection;
 487         }
 488
 489         /* calculate t, scale parameters, ray intersects triangle */
 490         scalar t = edge2 & qVec;
 491         scalar inv_det = 1.0 / det;
 492         t *= inv_det;
 493         u *= inv_det;
 494         v *= inv_det;
 495
 496         intersection.setHit();
 497         intersection.setPoint(a_ + u*edge1 + v*edge2);
 498         intersection.setDistance(t);
 499     }
 500     else if (alg == intersection::HALF_RAY || alg == intersection::FULL_RAY)
 501     {
 502         // Non-culling branch
 503         if (det > -SMALL && det < SMALL)
 504         {
 505             // return miss
 506             return intersection;
 507         }
 508         const scalar inv_det = 1.0 / det;
 509
 510         /* calculate distance from a_ to ray origin */
 511         const vector tVec = orig - a_;
 512         /* calculate U parameter and test bounds */
 513         const scalar u = (tVec & pVec)*inv_det;
 514
 515         if (u < 0.0 || u > 1.0)
 516         {
 517             // return miss
 518             return intersection;
 519         }
 520         /* prepare to test V parameter */
 521         const vector qVec = tVec ^ edge1;
 522         /* calculate V parameter and test bounds */
 523         const scalar v = (dir & qVec) * inv_det;
 524
 525         if (v < 0.0 || u + v > 1.0)
 526         {
 527             // return miss
 528             return intersection;
 529         }
 530         /* calculate t, ray intersects triangle */
 531         const scalar t = (edge2 & qVec) * inv_det;
 532
 533         if (alg == intersection::HALF_RAY && t < 0)
 534         {
 535             // return miss
 536             return intersection;
 537         }
 538
 539         intersection.setHit();
 540         intersection.setPoint(a_ + u*edge1 + v*edge2);
 541         intersection.setDistance(t);
 542     }
 543     else
 544     {
 545         FatalErrorIn
 546         (
 547             "triangle<Point, PointRef>::intersection(const point&"
 548             ", const vector&, const intersection::algorithm)"
 549         )   << "intersection only defined for VISIBLE, FULL_RAY or HALF_RAY"
 550             << abort(FatalError);
 551     }
 552
 553     return intersection;
 554 }
 555
 556
 557 template<class Point, class PointRef>
 558 inline pointHit triangle<Point, PointRef>::nearestPoint
 559 (
 560     const point& p
 561 ) const
 562 {
 563     // Express triangle in terms of baseVertex (point a_) and
 564     // two edge vectors
 565     vector E0 = b_ - a_;
 566     vector E1 = c_ - a_;
 567
 568     return nearestPoint(a_, E0, E1, p);
 569 }
 570
 571
 572 template<class Point, class PointRef>
 573 inline bool triangle<Point, PointRef>::classify
 574 (
 575     const point& p,
 576     const scalar tol,
 577     label& nearType,
 578     label& nearLabel
 579 ) const
 580 {
 581     const vector E0 = b_ - a_;
 582     const vector E1 = c_ - a_;
 583     const vector n = 0.5*(E0 ^ E1);
 584
 585     // Get largest component of normal
 586     scalar magX = Foam::mag(n.x());
 587     scalar magY = Foam::mag(n.y());
 588     scalar magZ = Foam::mag(n.z());
 589
 590     label i0 = -1;
 591     if ((magX >= magY) && (magX >= magZ))
 592     {
 593         i0 = 0;
 594     }
 595     else if ((magY >= magX) && (magY >= magZ))
 596     {
 597         i0 = 1;
 598     }
 599     else
 600     {
 601         i0 = 2;
 602     }
 603
 604     // Get other components
 605     label i1 = (i0 + 1) % 3;
 606     label i2 = (i1 + 1) % 3;
 607
 608
 609     scalar u1 = E0[i1];
 610     scalar v1 = E0[i2];
 611
 612     scalar u2 = E1[i1];
 613     scalar v2 = E1[i2];
 614
 615     scalar det = v2*u1 - u2*v1;
 616
 617     scalar u0 = p[i1] - a_[i1];
 618     scalar v0 = p[i2] - a_[i2];
 619
 620     scalar alpha = 0;
 621     scalar beta = 0;
 622
 623     bool hit = false;
 624
 625     if (Foam::mag(u1) < SMALL)
 626     {
 627         beta = u0/u2;
 628
 629         alpha = (v0 - beta*v2)/v1;
 630
 631         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 632     }
 633     else
 634     {
 635         beta = (v0*u1 - u0*v1)/det;
 636
 637         alpha = (u0 - beta*u2)/u1;
 638
 639         hit = ((alpha >= 0) && ((alpha + beta) <= 1));
 640     }
 641
 642     //
 643     // Now alpha, beta are the coordinates in the triangle local coordinate
 644     // system E0, E1
 645     //
 646
 647     nearType = NONE;
 648     nearLabel = -1;
 649
 650     if (Foam::mag(alpha+beta-1) <= tol)
 651     {
 652         // On edge between vert 1-2 (=edge 1)
 653
 654         if (Foam::mag(alpha) <= tol)
 655         {
 656             nearType = POINT;
 657             nearLabel = 2;
 658         }
 659         else if (Foam::mag(beta) <= tol)
 660         {
 661             nearType = POINT;
 662             nearLabel = 1;
 663         }
 664         else if ((alpha >= 0) && (alpha <= 1) && (beta >= 0) && (beta <= 1))
 665         {
 666             nearType = EDGE;
 667             nearLabel = 1;
 668         }
 669     }
 670     else if (Foam::mag(alpha) <= tol)
 671     {
 672         // On edge between vert 2-0 (=edge 2)
 673
 674         if (Foam::mag(beta) <= tol)
 675         {
 676             nearType = POINT;
 677             nearLabel = 0;
 678         }
 679         else if (Foam::mag(beta-1) <= tol)
 680         {
 681             nearType = POINT;
 682             nearLabel = 2;
 683         }
 684         else if ((beta >= 0) && (beta <= 1))
 685         {
 686             nearType = EDGE;
 687             nearLabel = 2;
 688         }
 689     }
 690     else if (Foam::mag(beta) <= tol)
 691     {
 692         // On edge between vert 0-1 (= edge 0)
 693
 694         if (Foam::mag(alpha) <= tol)
 695         {
 696             nearType = POINT;
 697             nearLabel = 0;
 698         }
 699         else if (Foam::mag(alpha-1) <= tol)
 700         {
 701             nearType = POINT;
 702             nearLabel = 1;
 703         }
 704         else if ((alpha >= 0) && (alpha <= 1))
 705         {
 706             nearType = EDGE;
 707             nearLabel = 0;
 708         }
 709     }
 710
 711     return hit;
 712 }
 713
 714
 715
 716
 717
 718 // * * * * * * * * * * * * * * * Ostream Operator  * * * * * * * * * * * * * //
 719
 720 template<class point, class pointRef>
 721 inline Istream& operator>>(Istream& is, triangle<point, pointRef>& t)
 722 {
 723     // Read beginning of triangle point pair
 724     is.readBegin("triangle");
 725
 726     is >> t.a_ >> t.b_ >> t.c_;
 727
 728     // Read end of triangle point pair
 729     is.readEnd("triangle");
 730
 731     // Check state of Ostream
 732     is.check("Istream& operator>>(Istream&, triangle&)");
 733
 734     return is;
 735 }
 736
 737
 738 template<class Point, class PointRef>
 739 inline Ostream& operator<<(Ostream& os, const triangle<Point, PointRef>& t)
 740 {
 741     os  << nl
 742         << token::BEGIN_LIST
 743         << t.a_ << token::SPACE << t.b_ << token::SPACE << t.c_
 744         << token::END_LIST;
 745
 746     return os;
 747 }
 748
 749
 750 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 751
 752 } // End namespace Foam
 753
 754 // ************************************************************************* //
 755