triangleI.H 15.4 KB
Newer Older
1
2
3
4
/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   \\    /   O peration     |
5
    \\  /    A nd           | Copyright (C) 1991-2010 OpenCFD Ltd.
6
7
8
9
10
     \\/     M anipulation  |
-------------------------------------------------------------------------------
License
    This file is part of OpenFOAM.

11
12
13
14
    OpenFOAM is free software: you can redistribute it and/or modify it
    under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
15
16
17
18
19
20
21

    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
    for more details.

    You should have received a copy of the GNU General Public License
22
    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
23
24
25
26
27

\*---------------------------------------------------------------------------*/

#include "IOstreams.H"
#include "pointHit.H"
28
#include "mathematicalConstants.H"
29
30
31
32

// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //

template<class Point, class PointRef>
33
inline Foam::triangle<Point, PointRef>::triangle
34
35
36
37
38
39
40
41
42
43
44
45
46
(
    const Point& a,
    const Point& b,
    const Point& c
)
:
    a_(a),
    b_(b),
    c_(c)
{}


template<class Point, class PointRef>
47
48
49
50
51
52
53
54
55
56
inline Foam::triangle<Point, PointRef>::triangle
(
    const UList<Point>& points,
    const FixedList<label, 3>& indices
)
:
    a_(points[indices[0]]),
    b_(points[indices[1]]),
    c_(points[indices[2]])
{}
57
58
59



60
61
62
63
template<class Point, class PointRef>
inline Foam::triangle<Point, PointRef>::triangle(Istream& is)
{
    is  >> *this;
64
65
66
67
68
69
}


// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //

template<class Point, class PointRef>
70
inline const Point& Foam::triangle<Point, PointRef>::a() const
71
72
73
74
75
{
    return a_;
}

template<class Point, class PointRef>
76
inline const Point& Foam::triangle<Point, PointRef>::b() const
77
78
79
80
81
{
    return b_;
}

template<class Point, class PointRef>
82
inline const Point& Foam::triangle<Point, PointRef>::c() const
83
84
85
86
87
88
{
    return c_;
}


template<class Point, class PointRef>
89
inline Point Foam::triangle<Point, PointRef>::centre() const
90
91
92
93
94
95
{
    return (1.0/3.0)*(a_ + b_ + c_);
}


template<class Point, class PointRef>
96
inline Foam::scalar Foam::triangle<Point, PointRef>::mag() const
97
{
98
    return Foam::mag(normal());
99
100
101
102
}


template<class Point, class PointRef>
103
inline Foam::vector Foam::triangle<Point, PointRef>::normal() const
104
105
106
107
108
109
{
    return 0.5*((b_ - a_)^(c_ - a_));
}


template<class Point, class PointRef>
110
inline Point Foam::triangle<Point, PointRef>::circumCentre() const
111
112
113
114
115
116
117
118
119
120
121
{
    scalar d1 = (c_ - a_)&(b_ - a_);
    scalar d2 = -(c_ - b_)&(b_ - a_);
    scalar d3 = (c_ - a_)&(c_ - b_);

    scalar c1 = d2*d3;
    scalar c2 = d3*d1;
    scalar c3 = d1*d2;

    scalar c = c1 + c2 + c3;

122
    return
123
124
125
126
127
128
129
    (
        ((c2 + c3)*a_ + (c3 + c1)*b_ + (c1 + c2)*c_)/(2*c)
    );
}


template<class Point, class PointRef>
130
inline Foam::scalar Foam::triangle<Point, PointRef>::circumRadius() const
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
{
    scalar d1 = (c_ - a_) & (b_ - a_);
    scalar d2 = - (c_ - b_) & (b_ - a_);
    scalar d3 = (c_ - a_) & (c_ - b_);

    scalar denom = d2*d3 + d3*d1 + d1*d2;

    if (Foam::mag(denom) < VSMALL)
    {
        return GREAT;
    }
    else
    {
        scalar a = (d1 + d2)*(d2 + d3)*(d3 + d1) / denom;

        return 0.5*Foam::sqrt(min(GREAT, max(0, a)));
    }
}


template<class Point, class PointRef>
152
inline Foam::scalar Foam::triangle<Point, PointRef>::quality() const
153
{
154
155
    // Note: 3*sqr(3)/(4*pi) = 0.4134966716

156
157
158
    return
        mag()
      / (
159
            constant::mathematical::pi
160
           *Foam::sqr(circumRadius())
161
           *0.4134966716
162
163
164
165
166
167
          + VSMALL
        );
}


template<class Point, class PointRef>
168
169
170
171
inline Foam::scalar Foam::triangle<Point, PointRef>::sweptVol
(
    const triangle& t
) const
172
{
173
    return (1.0/12.0)*
174
175
176
177
    (
        ((t.a_ - a_) & ((b_ - a_)^(c_ - a_)))
      + ((t.b_ - b_) & ((c_ - b_)^(t.a_ - b_)))
      + ((c_ - t.c_) & ((t.b_ - t.c_)^(t.a_ - t.c_)))
178
179
180
181

      + ((t.a_ - a_) & ((b_ - a_)^(c_ - a_)))
      + ((b_ - t.b_) & ((t.a_ - t.b_)^(t.c_ - t.b_)))
      + ((c_ - t.c_) & ((b_ - t.c_)^(t.a_ - t.c_)))
182
183
184
185
    );
}


186
template<class Point, class PointRef>
187
inline Foam::tensor Foam::triangle<Point, PointRef>::inertia
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
(
    PointRef refPt,
    scalar density
) const
{
    Point aRel = a_ - refPt;
    Point bRel = b_ - refPt;
    Point cRel = c_ - refPt;

    tensor V
    (
        aRel.x(), aRel.y(), aRel.z(),
        bRel.x(), bRel.y(), bRel.z(),
        cRel.x(), cRel.y(), cRel.z()
    );

    scalar a = Foam::mag((b_ - a_)^(c_ - a_));

    tensor S = 1/24.0*(tensor::one + I);

    return
    (
        a*I/24.0*
        (
            (aRel & aRel)
          + (bRel & bRel)
          + (cRel & cRel)
          + ((aRel + bRel + cRel) & (aRel + bRel + cRel))
        )
      - a*(V.T() & S & V)
    )
   *density;
}

222
223

template<class Point, class PointRef>
224
inline Point Foam::triangle<Point, PointRef>::randomPoint(Random& rndGen) const
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
{
    // Generating Random Points in Triangles
    // by Greg Turk
    // from "Graphics Gems", Academic Press, 1990
    // http://tog.acm.org/GraphicsGems/gems/TriPoints.c

    scalar s = rndGen.scalar01();

    scalar t = sqrt(rndGen.scalar01());

    return (1 - t)*a_ + (1 - s)*t*b_ + s*t*c_;
}


template<class Point, class PointRef>
240
Foam::scalar Foam::triangle<Point, PointRef>::barycentric
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
(
    const point& pt,
    List<scalar>& bary
) const
{
    // From:
    // Real-time collision detection, Christer Ericson, 2005, p47-48

    vector v0 = b_ - a_;
    vector v1 = c_ - a_;
    vector v2 = pt - a_;

    scalar d00 = v0 & v0;
    scalar d01 = v0 & v1;
    scalar d11 = v1 & v1;
    scalar d20 = v2 & v0;
    scalar d21 = v2 & v1;

    scalar denom = d00*d11 - d01*d01;

    if (Foam::mag(denom) < SMALL)
    {
        WarningIn
        (
            "List<scalar> triangle<Point, PointRef>::barycentric"
            "("
                "const point& pt"
            ") const"
        )
            << "Degenerate triangle - returning 1/3 barycentric coordinates."
            << endl;

        bary = List<scalar>(3, 1.0/3.0);

        return denom;
    }

    bary.setSize(3);

    bary[0] = (d11*d20 - d01*d21)/denom;
    bary[1] = (d00*d21 - d01*d20)/denom;
    bary[2] = 1.0 - bary[0] - bary[1];

    return denom;
}


288
template<class Point, class PointRef>
289
inline Foam::pointHit Foam::triangle<Point, PointRef>::ray
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
(
    const point& p,
    const vector& q,
    const intersection::algorithm alg,
    const intersection::direction dir
) const
{
    // Express triangle in terms of baseVertex (point a_) and
    // two edge vectors
    vector E0 = b_ - a_;
    vector E1 = c_ - a_;

    // Initialize intersection to miss.
    pointHit inter(p);

    vector n(0.5*(E0 ^ E1));
    scalar area = Foam::mag(n);

    if (area < VSMALL)
    {
        // Ineligible miss.
        inter.setMiss(false);

        // The miss point is the nearest point on the triangle. Take any one.
        inter.setPoint(a_);

        // The distance to the miss is the distance between the
        // original point and plane of intersection. No normal so use
        // distance from p to a_
        inter.setDistance(Foam::mag(a_ - p));

        return inter;
    }

    vector q1 = q/Foam::mag(q);

    if (dir == intersection::CONTACT_SPHERE)
    {
mattijs's avatar
mattijs committed
328
329
        n /= area;

330
331
332
        return ray(p, q1 - n, alg, intersection::VECTOR);
    }

333
    // Intersection point with triangle plane
334
    point pInter;
335
    // Is intersection point inside triangle
mattijs's avatar
mattijs committed
336
337
338
339
340
341
    bool hit;
    {
        // Reuse the fast ray intersection routine below in FULL_RAY
        // mode since the original intersection routine has rounding problems.
        pointHit fastInter = intersection(p, q1, intersection::FULL_RAY);
        hit = fastInter.hit();
342
343
344
345
346
347
348
349
350
351
352

        if (hit)
        {
            pInter = fastInter.rawPoint();
        }
        else
        {
            // Calculate intersection of ray with triangle plane
            vector v = a_ - p;
            pInter = p + (q1&v)*q1;
        }
mattijs's avatar
mattijs committed
353
    }
354

355
    // Distance to intersection point
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
    scalar dist = q1 & (pInter - p);

    const scalar planarPointTol =
        Foam::min
        (
            Foam::min
            (
                Foam::mag(E0),
                Foam::mag(E1)
            ),
            Foam::mag(c_ - b_)
        )*intersection::planarTol();

    bool eligible =
        alg == intersection::FULL_RAY
371
     || (alg == intersection::HALF_RAY && dist > -planarPointTol)
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
     || (
            alg == intersection::VISIBLE
         && ((q1 & normal()) < -VSMALL)
        );

    if (hit && eligible)
    {
        // Hit. Set distance to intersection.
        inter.setHit();
        inter.setPoint(pInter);
        inter.setDistance(dist);
    }
    else
    {
        // Miss or ineligible hit.
        inter.setMiss(eligible);

        // The miss point is the nearest point on the triangle
390
        inter.setPoint(nearestPoint(p).rawPoint());
391
392
393
394
395
396

        // The distance to the miss is the distance between the
        // original point and plane of intersection
        inter.setDistance(Foam::mag(pInter - p));
    }

mattijs's avatar
mattijs committed
397

398
399
400
401
    return inter;
}


mattijs's avatar
mattijs committed
402
403
404
// From "Fast, Minimum Storage Ray/Triangle Intersection"
// Moeller/Trumbore.
template<class Point, class PointRef>
405
inline Foam::pointHit Foam::triangle<Point, PointRef>::intersection
mattijs's avatar
mattijs committed
406
407
(
    const point& orig,
408
    const vector& dir,
mattijs's avatar
mattijs committed
409
410
    const intersection::algorithm alg,
    const scalar tol
mattijs's avatar
mattijs committed
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
) const
{
    const vector edge1 = b_ - a_;
    const vector edge2 = c_ - a_;

    // begin calculating determinant - also used to calculate U parameter
    const vector pVec = dir ^ edge2;

    // if determinant is near zero, ray lies in plane of triangle
    const scalar det = edge1 & pVec;

    // Initialise to miss
    pointHit intersection(false, vector::zero, GREAT, false);

    if (alg == intersection::VISIBLE)
    {
        // Culling branch
mattijs's avatar
mattijs committed
428
        if (det < ROOTVSMALL)
mattijs's avatar
mattijs committed
429
        {
mattijs's avatar
mattijs committed
430
            // Ray on wrong side of triangle. Return miss
mattijs's avatar
mattijs committed
431
432
433
434
435
436
            return intersection;
        }
    }
    else if (alg == intersection::HALF_RAY || alg == intersection::FULL_RAY)
    {
        // Non-culling branch
mattijs's avatar
mattijs committed
437
        if (det > -ROOTVSMALL && det < ROOTVSMALL)
mattijs's avatar
mattijs committed
438
        {
mattijs's avatar
mattijs committed
439
            // Ray parallel to triangle. Return miss
mattijs's avatar
mattijs committed
440
441
            return intersection;
        }
mattijs's avatar
mattijs committed
442
    }
mattijs's avatar
mattijs committed
443

mattijs's avatar
mattijs committed
444
    const scalar inv_det = 1.0 / det;
mattijs's avatar
mattijs committed
445

mattijs's avatar
mattijs committed
446
447
    /* calculate distance from a_ to ray origin */
    const vector tVec = orig-a_;
mattijs's avatar
mattijs committed
448

mattijs's avatar
mattijs committed
449
450
    /* calculate U parameter and test bounds */
    const scalar u = (tVec & pVec)*inv_det;
mattijs's avatar
mattijs committed
451

mattijs's avatar
mattijs committed
452
453
454
455
456
    if (u < -tol || u > 1.0+tol)
    {
        // return miss
        return intersection;
    }
mattijs's avatar
mattijs committed
457

mattijs's avatar
mattijs committed
458
459
460
461
462
463
464
465
466
467
    /* prepare to test V parameter */
    const vector qVec = tVec ^ edge1;

    /* calculate V parameter and test bounds */
    const scalar v = (dir & qVec) * inv_det;

    if (v < -tol || u + v > 1.0+tol)
    {
        // return miss
        return intersection;
mattijs's avatar
mattijs committed
468
    }
mattijs's avatar
mattijs committed
469
470
471
472
473

    /* calculate t, scale parameters, ray intersects triangle */
    const scalar t = (edge2 & qVec) * inv_det;

    if (alg == intersection::HALF_RAY && t < -tol)
mattijs's avatar
mattijs committed
474
    {
mattijs's avatar
mattijs committed
475
476
        // Wrong side of orig. Return miss
        return intersection;
mattijs's avatar
mattijs committed
477
478
    }

mattijs's avatar
mattijs committed
479
480
481
482
    intersection.setHit();
    intersection.setPoint(a_ + u*edge1 + v*edge2);
    intersection.setDistance(t);

mattijs's avatar
mattijs committed
483
484
485
    return intersection;
}

486
487

template<class Point, class PointRef>
488
Foam::pointHit Foam::triangle<Point, PointRef>::nearestPointClassify
489
490
491
492
493
494
(
    const point& p,
    label& nearType,
    label& nearLabel
) const
{
495
496
    // Adapted from:
    // Real-time collision detection, Christer Ericson, 2005, 136-142
497

498
499
500
501
    // Check if P in vertex region outside A
    vector ab = b_ - a_;
    vector ac = c_ - a_;
    vector ap = p - a_;
502

503
504
    scalar d1 = ab & ap;
    scalar d2 = ac & ap;
505

506
507
508
    if (d1 <= 0.0 && d2 <= 0.0)
    {
        // barycentric coordinates (1, 0, 0)
509

510
511
512
513
        nearType = POINT;
        nearLabel = 0;
        return pointHit(false, a_, Foam::mag(a_ - p), true);
    }
514

515
516
517
518
    // Check if P in vertex region outside B
    vector bp = p - b_;
    scalar d3 = ab & bp;
    scalar d4 = ac & bp;
519

520
521
522
    if (d3 >= 0.0 && d4 <= d3)
    {
        // barycentric coordinates (0, 1, 0)
523

524
525
526
527
        nearType = POINT;
        nearLabel = 1;
        return pointHit(false, b_, Foam::mag(b_ - p), true);
    }
528

529
530
    // Check if P in edge region of AB, if so return projection of P onto AB
    scalar vc = d1*d4 - d3*d2;
531

532
    if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0)
533
    {
534
535
        // barycentric coordinates (1-v, v, 0)
        scalar v = d1/(d1 - d3);
536

537
538
539
540
        point nearPt =  a_ + v*ab;
        nearType = EDGE;
        nearLabel = 0;
        return pointHit(false, nearPt, Foam::mag(nearPt - p), true);
541
542
    }

543
544
545
546
    // Check if P in vertex region outside C
    vector cp = p - c_;
    scalar d5 = ab & cp;
    scalar d6 = ac & cp;
547

548
549
550
    if (d6 >= 0.0 && d5 <= d6)
    {
        // barycentric coordinates (0, 0, 1)
551

552
553
554
555
        nearType = POINT;
        nearLabel = 2;
        return pointHit(false, c_, Foam::mag(c_ - p), true);
    }
556

557
558
    // Check if P in edge region of AC, if so return projection of P onto AC
    scalar vb = d5*d2 - d1*d6;
mattijs's avatar
mattijs committed
559

560
    if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0)
mattijs's avatar
mattijs committed
561
    {
562
563
564
565
566
567
568
        // barycentric coordinates (1-w, 0, w)
        scalar w = d2/(d2 - d6);

        point nearPt = a_ + w*ac;
        nearType = EDGE;
        nearLabel = 2;
        return pointHit(false, nearPt, Foam::mag(nearPt - p), true);
mattijs's avatar
mattijs committed
569
570
    }

571
572
    // Check if P in edge region of BC, if so return projection of P onto BC
    scalar va = d3*d6 - d5*d4;
573

574
    if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0)
575
    {
576
577
        // barycentric coordinates (0, 1-w, w)
        scalar w = (d4 - d3)/((d4 - d3) + (d5 - d6));
578

579
580
581
582
        point nearPt = b_ + w*(c_ - b_);
        nearType = EDGE;
        nearLabel = 1;
        return pointHit(false, nearPt, Foam::mag(nearPt - p), true);
583
584
    }

585
586
587
588
589
    // P inside face region. Compute Q through its barycentric
    // coordinates (u, v, w)
    scalar denom = 1.0/(va + vb + vc);
    scalar v = vb * denom;
    scalar w = vc * denom;
590

591
    // = u*a + v*b + w*c, u = va*denom = 1.0 - v - w
592

593
594
595
596
    point nearPt = a_ + ab*v + ac*w;
    nearType = NONE,
    nearLabel = -1;
    return pointHit(true, nearPt, Foam::mag(nearPt - p), false);
597
598
599
}


600
template<class Point, class PointRef>
601
inline Foam::pointHit Foam::triangle<Point, PointRef>::nearestPoint
602
603
604
605
606
607
608
609
610
611
(
    const point& p
) const
{
    // Dummy labels
    label nearType = -1;
    label nearLabel = -1;

    return nearestPointClassify(p, nearType, nearLabel);
}
612
613


614
template<class Point, class PointRef>
615
inline bool Foam::triangle<Point, PointRef>::classify
616
617
618
619
620
621
622
623
624
(
    const point& p,
    label& nearType,
    label& nearLabel
) const
{
    return nearestPointClassify(p, nearType, nearLabel).hit();
}

625
626
627

// * * * * * * * * * * * * * * * Ostream Operator  * * * * * * * * * * * * * //

628
template<class Point, class PointRef>
629
630
inline Foam::Istream& Foam::operator>>
(
631
632
    Istream& is,
    triangle<Point, PointRef>& t
633
)
634
635
{
    is.readBegin("triangle");
636
    is  >> t.a_ >> t.b_ >> t.c_;
637
638
639
640
641
642
643
644
    is.readEnd("triangle");

    is.check("Istream& operator>>(Istream&, triangle&)");
    return is;
}


template<class Point, class PointRef>
645
646
647
648
649
inline Foam::Ostream& Foam::operator<<
(
    Ostream& os,
    const triangle<Point, PointRef>& t
)
650
651
652
{
    os  << nl
        << token::BEGIN_LIST
653
654
655
        << t.a_ << token::SPACE
        << t.b_ << token::SPACE
        << t.c_
656
657
658
659
660
661
662
        << token::END_LIST;

    return os;
}


// ************************************************************************* //