FreeCAD

Elements.cpp
1627 строк · 49.5 Кб
Перенос по словам
1
/***************************************************************************
2
 *   Copyright (c) 2005 Imetric 3D GmbH                                    *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  is distributed in the hope that it will be useful,      *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
14
 *   GNU Library General Public License for more details.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23
#include "PreCompiled.h"
24

25
#include <Mod/Mesh/App/WildMagic4/Wm4DistSegment3Triangle3.h>
26
#include <Mod/Mesh/App/WildMagic4/Wm4DistVector3Triangle3.h>
27
#include <Mod/Mesh/App/WildMagic4/Wm4IntrSegment3Box3.h>
28
#include <Mod/Mesh/App/WildMagic4/Wm4IntrSegment3Plane3.h>
29

30
#include "Algorithm.h"
31
#include "Elements.h"
32
#include "Utilities.h"
33
#include "tritritest.h"
34

35

36
using namespace MeshCore;
37
using namespace Wm4;
38

39
MeshPointArray::MeshPointArray(const MeshPointArray& ary) = default;
40

41
MeshPointArray::MeshPointArray(MeshPointArray&& ary) = default;
42

43
PointIndex MeshPointArray::Get(const MeshPoint& rclPoint)
44
{
45
    iterator clIter;
46

47
    clIter = std::find(begin(), end(), rclPoint);
48
    if (clIter != end()) {
49
        return clIter - begin();
50
    }
51
    else {
52
        return POINT_INDEX_MAX;
53
    }
54
}
55

56
PointIndex MeshPointArray::GetOrAddIndex(const MeshPoint& rclPoint)
57
{
58
    PointIndex ulIndex {};
59

60
    if ((ulIndex = Get(rclPoint)) == POINT_INDEX_MAX) {
61
        push_back(rclPoint);
62
        return static_cast<PointIndex>(size() - 1);
63
    }
64
    else {
65
        return ulIndex;
66
    }
67
}
68

69
void MeshPointArray::SetFlag(MeshPoint::TFlagType tF) const
70
{
71
    for (MeshPointArray::_TConstIterator i = begin(); i < end(); ++i) {
72
        i->SetFlag(tF);
73
    }
74
}
75

76
void MeshPointArray::ResetFlag(MeshPoint::TFlagType tF) const
77
{
78
    for (MeshPointArray::_TConstIterator i = begin(); i < end(); ++i) {
79
        i->ResetFlag(tF);
80
    }
81
}
82

83
void MeshPointArray::SetProperty(unsigned long ulVal) const
84
{
85
    for (const auto& pP : *this) {
86
        pP.SetProperty(ulVal);
87
    }
88
}
89

90
void MeshPointArray::ResetInvalid() const
91
{
92
    for (const auto& pP : *this) {
93
        pP.ResetInvalid();
94
    }
95
}
96

97
MeshPointArray& MeshPointArray::operator=(const MeshPointArray& rclPAry) = default;
98

99
MeshPointArray& MeshPointArray::operator=(MeshPointArray&& rclPAry) = default;
100

101
void MeshPointArray::Transform(const Base::Matrix4D& mat)
102
{
103
    for (auto& pP : *this) {
104
        mat.multVec(pP, pP);
105
    }
106
}
107

108
MeshFacetArray::MeshFacetArray(const MeshFacetArray& ary) = default;
109

110
MeshFacetArray::MeshFacetArray(MeshFacetArray&& ary) = default;
111

112
void MeshFacetArray::Erase(_TIterator pIter)
113
{
114
    FacetIndex i {}, *pulN {};
115
    _TIterator pPass, pEnd;
116
    FacetIndex ulInd = pIter - begin();
117
    erase(pIter);
118
    pPass = begin();
119
    pEnd = end();
120
    while (pPass < pEnd) {
121
        for (i = 0; i < 3; i++) {
122
            pulN = &pPass->_aulNeighbours[i];
123
            if ((*pulN > ulInd) && (*pulN != FACET_INDEX_MAX)) {
124
                (*pulN)--;
125
            }
126
        }
127
        pPass++;
128
    }
129
}
130

131
void MeshFacetArray::TransposeIndices(PointIndex ulOrig, PointIndex ulNew)
132
{
133
    _TIterator pIter = begin(), pEnd = end();
134

135
    while (pIter < pEnd) {
136
        pIter->Transpose(ulOrig, ulNew);
137
        ++pIter;
138
    }
139
}
140

141
void MeshFacetArray::DecrementIndices(PointIndex ulIndex)
142
{
143
    _TIterator pIter = begin(), pEnd = end();
144

145
    while (pIter < pEnd) {
146
        pIter->Decrement(ulIndex);
147
        ++pIter;
148
    }
149
}
150

151
void MeshFacetArray::SetFlag(MeshFacet::TFlagType tF) const
152
{
153
    for (MeshFacetArray::_TConstIterator i = begin(); i < end(); ++i) {
154
        i->SetFlag(tF);
155
    }
156
}
157

158
void MeshFacetArray::ResetFlag(MeshFacet::TFlagType tF) const
159
{
160
    for (MeshFacetArray::_TConstIterator i = begin(); i < end(); ++i) {
161
        i->ResetFlag(tF);
162
    }
163
}
164

165
void MeshFacetArray::SetProperty(unsigned long ulVal) const
166
{
167
    for (const auto& pF : *this) {
168
        pF.SetProperty(ulVal);
169
    }
170
}
171

172
void MeshFacetArray::ResetInvalid() const
173
{
174
    for (const auto& pF : *this) {
175
        pF.ResetInvalid();
176
    }
177
}
178

179
MeshFacetArray& MeshFacetArray::operator=(const MeshFacetArray& rclFAry) = default;
180

181
MeshFacetArray& MeshFacetArray::operator=(MeshFacetArray&& rclFAry) = default;
182

183
// -----------------------------------------------------------------
184

185
bool MeshGeomEdge::ContainedByOrIntersectBoundingBox(const Base::BoundBox3f& rclBB) const
186
{
187
    // Test whether all corner points of the Edge are on one of the 6 sides of the BB
188
    if (!(GetBoundBox() && rclBB)) {
189
        return false;
190
    }
191

192
    // Test whether Edge-BB is completely in BB
193
    if (rclBB.IsInBox(GetBoundBox())) {
194
        return true;
195
    }
196

197
    // Test whether one of the corner points is in BB
198
    for (const auto& pnt : _aclPoints) {
199
        if (rclBB.IsInBox(pnt)) {
200
            return true;
201
        }
202
    }
203

204
    // "real" test for cut
205
    if (IntersectBoundingBox(rclBB)) {
206
        return true;
207
    }
208

209
    return false;
210
}
211

212
Base::BoundBox3f MeshGeomEdge::GetBoundBox() const
213
{
214
    return {_aclPoints, 2};
215
}
216

217
bool MeshGeomEdge::IntersectBoundingBox(const Base::BoundBox3f& rclBB) const
218
{
219
    const Base::Vector3f& rclP0 = _aclPoints[0];
220
    const Base::Vector3f& rclP1 = _aclPoints[1];
221

222
    Vector3<float> A(rclP0.x, rclP0.y, rclP0.z);
223
    Vector3<float> B(rclP1.x, rclP1.y, rclP1.z);
224

225
    Vector3<float> n = B - A;
226
    float len = n.Length();
227
    n.Normalize();
228
    Vector3<float> p = 0.5f * (A + B);
229

230
    Segment3<float> akSeg(p, n, 0.5f * len);
231

232
    Base::Vector3f clCenter = rclBB.GetCenter();
233
    Vector3<float> center(clCenter.x, clCenter.y, clCenter.z);
234
    Vector3<float> axis0(1.0f, 0.0f, 0.0f);
235
    Vector3<float> axis1(0.0f, 1.0f, 0.0f);
236
    Vector3<float> axis2(0.0f, 0.0f, 1.0f);
237
    float extent0 = 0.5f * rclBB.LengthX();
238
    float extent1 = 0.5f * rclBB.LengthY();
239
    float extent2 = 0.5f * rclBB.LengthZ();
240

241
    Box3<float> kBox(center, axis0, axis1, axis2, extent0, extent1, extent2);
242

243
    IntrSegment3Box3<float> intrsectbox(akSeg, kBox, false);
244
    return intrsectbox.Test();
245
}
246

247
bool MeshGeomEdge::IntersectWithLine(const Base::Vector3f& rclPt,
248
                                     const Base::Vector3f& rclDir,
249
                                     Base::Vector3f& rclRes) const
250
{
251
    const float eps = 1e-06f;
252
    Base::Vector3f n = _aclPoints[1] - _aclPoints[0];
253

254
    // check angle between edge and the line direction, FLOAT_MAX is
255
    // returned for degenerated edges
256
    float fAngle = rclDir.GetAngle(n);
257
    if (fAngle == 0) {
258
        // parallel lines
259
        float distance = _aclPoints[0].DistanceToLine(rclPt, rclDir);
260
        if (distance < eps) {
261
            // lines are equal
262
            rclRes = _aclPoints[0];
263
            return true;
264
        }
265

266
        return false;  // no intersection possible
267
    }
268

269
    // that's the normal of a helper plane and its base at _aclPoints
270
    Base::Vector3f normal = n.Cross(rclDir);
271

272
    // if the distance of rclPt to the plane is higher than eps then the
273
    // two lines are warped and there is no intersection possible
274
    if (fabs(rclPt.DistanceToPlane(_aclPoints[0], normal)) > eps) {
275
        return false;
276
    }
277

278
    // get a second helper plane and get the intersection with the line
279
    Base::Vector3f normal2 = normal.Cross(n);
280

281
    float s = ((_aclPoints[0] - rclPt) * normal2) / (rclDir * normal2);
282
    rclRes = rclPt + s * rclDir;
283

284
    float dist1 = Base::Distance(_aclPoints[0], _aclPoints[1]);
285
    float dist2 = Base::Distance(_aclPoints[0], rclRes);
286
    float dist3 = Base::Distance(_aclPoints[1], rclRes);
287

288
    return dist2 + dist3 <= dist1 + eps;
289
}
290

291
bool MeshGeomEdge::IsParallel(const MeshGeomEdge& edge) const
292
{
293
    Base::Vector3f r(_aclPoints[1] - _aclPoints[0]);
294
    Base::Vector3f s(edge._aclPoints[1] - edge._aclPoints[0]);
295
    Base::Vector3f n = r.Cross(s);
296
    return n.IsNull();
297
}
298

299
bool MeshGeomEdge::IsCollinear(const MeshGeomEdge& edge) const
300
{
301
    if (IsParallel(edge)) {
302
        Base::Vector3f r(_aclPoints[1] - _aclPoints[0]);
303
        Base::Vector3f d = edge._aclPoints[0] - _aclPoints[0];
304
        return d.Cross(r).IsNull();
305
    }
306

307
    return false;
308
}
309

310
bool MeshGeomEdge::IntersectWithEdge(const MeshGeomEdge& edge, Base::Vector3f& res) const
311
{
312
    const float eps = 1e-06f;
313
    Base::Vector3f p(_aclPoints[0]);
314
    Base::Vector3f r(_aclPoints[1] - _aclPoints[0]);
315
    Base::Vector3f q(edge._aclPoints[0]);
316
    Base::Vector3f s(edge._aclPoints[1] - edge._aclPoints[0]);
317
    Base::Vector3f n = r.Cross(s);
318
    Base::Vector3f d = q - p;
319

320
    // lines are collinear or parallel
321
    if (n.IsNull()) {
322
        if (d.Cross(r).IsNull()) {
323
            // Collinear
324
            if (IsProjectionPointOf(edge._aclPoints[0])) {
325
                res = edge._aclPoints[0];
326
                return true;
327
            }
328
            if (IsProjectionPointOf(edge._aclPoints[1])) {
329
                res = edge._aclPoints[1];
330
                return true;
331
            }
332

333
            return false;
334
        }
335
        else {
336
            // Parallel
337
            return false;
338
        }
339
    }
340
    else {
341
        // Get the distance of q to the plane defined by p and n
342
        float distance = q.DistanceToPlane(p, n);
343

344
        // lines are warped
345
        if (fabs(distance) > eps) {
346
            return false;
347
        }
348

349
        float t = d.Cross(s).Dot(n) / n.Sqr();
350
        float u = d.Cross(r).Dot(n) / n.Sqr();
351

352
        auto is_in_range = [](float v) {
353
            return v >= 0.0f && v <= 1.0f;
354
        };
355

356
        if (is_in_range(t) && is_in_range(u)) {
357
            res = p + t * r;  // equal to q + u * s
358
            return true;
359
        }
360

361
        return false;
362
    }
363
}
364

365
bool MeshGeomEdge::IntersectWithPlane(const Base::Vector3f& rclPt,
366
                                      const Base::Vector3f& rclDir,
367
                                      Base::Vector3f& rclRes) const
368
{
369
    float dist1 = _aclPoints[0].DistanceToPlane(rclPt, rclDir);
370
    float dist2 = _aclPoints[1].DistanceToPlane(rclPt, rclDir);
371

372
    // either both points are below or above the plane
373
    if (dist1 * dist2 >= 0.0f) {
374
        return false;
375
    }
376

377
    Base::Vector3f u = _aclPoints[1] - _aclPoints[0];
378
    Base::Vector3f b = rclPt - _aclPoints[0];
379
    float t = b.Dot(rclDir) / u.Dot(rclDir);
380
    rclRes = _aclPoints[0] + t * u;
381

382
    return true;
383
}
384

385
void MeshGeomEdge::ProjectPointToLine(const Base::Vector3f& rclPoint, Base::Vector3f& rclProj) const
386
{
387
    Base::Vector3f pt1 = rclPoint - _aclPoints[0];
388
    Base::Vector3f dir = _aclPoints[1] - _aclPoints[0];
389
    Base::Vector3f vec;
390
    vec.ProjectToLine(pt1, dir);
391
    rclProj = rclPoint + vec;
392
}
393

394
void MeshGeomEdge::ClosestPointsToLine(const Base::Vector3f& linePt,
395
                                       const Base::Vector3f& lineDir,
396
                                       Base::Vector3f& rclPnt1,
397
                                       Base::Vector3f& rclPnt2) const
398
{
399
    const float eps = 1e-06f;
400
    Base::Vector3f edgeDir = _aclPoints[1] - _aclPoints[0];
401

402
    // check angle between edge and the line direction, FLOAT_MAX is
403
    // returned for degenerated edges
404
    float fAngle = lineDir.GetAngle(edgeDir);
405
    if (fAngle == 0) {
406
        // parallel lines
407
        float distance = _aclPoints[0].DistanceToLine(linePt, lineDir);
408
        if (distance < eps) {
409
            // lines are equal
410
            rclPnt1 = _aclPoints[0];
411
            rclPnt2 = _aclPoints[0];
412
        }
413
        else {
414
            rclPnt1 = _aclPoints[0];
415
            MeshGeomEdge edge;
416
            edge._aclPoints[0] = linePt;
417
            edge._aclPoints[1] = linePt + lineDir;
418
            edge.ProjectPointToLine(rclPnt1, rclPnt2);
419
        }
420
    }
421
    else {
422
        // that's the normal of a helper plane
423
        Base::Vector3f normal = edgeDir.Cross(lineDir);
424

425
        // get a second helper plane and get the intersection with the line
426
        Base::Vector3f normal2 = normal.Cross(edgeDir);
427
        float s = ((_aclPoints[0] - linePt) * normal2) / (lineDir * normal2);
428
        rclPnt2 = linePt + s * lineDir;
429

430
        // get a third helper plane and get the intersection with the line
431
        Base::Vector3f normal3 = normal.Cross(lineDir);
432
        float t = ((linePt - _aclPoints[0]) * normal3) / (edgeDir * normal3);
433
        rclPnt1 = _aclPoints[0] + t * edgeDir;
434
    }
435
}
436

437
bool MeshGeomEdge::IsPointOf(const Base::Vector3f& rclPoint, float fDistance) const
438
{
439
    float len2 = Base::DistanceP2(_aclPoints[0], _aclPoints[1]);
440
    if (len2 == 0.0f) {
441
        return _aclPoints[0].IsEqual(rclPoint, 0.0f);
442
    }
443

444
    Base::Vector3f p2p1 = _aclPoints[1] - _aclPoints[0];
445
    Base::Vector3f pXp1 = rclPoint - _aclPoints[0];
446

447
    float dot = pXp1 * p2p1;
448
    float t = dot / len2;
449
    if (t < 0.0f || t > 1.0f) {
450
        return false;
451
    }
452

453
    // point on the edge
454
    Base::Vector3f ptEdge = t * p2p1 + _aclPoints[0];
455
    return Base::Distance(ptEdge, rclPoint) <= fDistance;
456
}
457

458
bool MeshGeomEdge::IsProjectionPointOf(const Base::Vector3f& point) const
459
{
460
    Base::Vector3f fromStartToPoint = point - _aclPoints[0];
461
    Base::Vector3f fromPointToEnd = _aclPoints[1] - point;
462
    float dot = fromStartToPoint * fromPointToEnd;
463
    return dot >= 0.0f;
464
}
465

466
// -----------------------------------------------------------------
467

468
MeshGeomFacet::MeshGeomFacet()
469
    : _bNormalCalculated(false)
470
    , _ucFlag(0)
471
    , _ulProp(0)
472
{}
473

474

475
MeshGeomFacet::MeshGeomFacet(const Base::Vector3f& v1,
476
                             const Base::Vector3f& v2,
477
                             const Base::Vector3f& v3)
478
    : _bNormalCalculated(false)
479
    , _ucFlag(0)
480
    , _ulProp(0)
481
{
482
    _aclPoints[0] = v1;
483
    _aclPoints[1] = v2;
484
    _aclPoints[2] = v3;
485
}
486

487

488
bool MeshGeomFacet::IsPointOf(const Base::Vector3f& rclPoint, float fDistance) const
489
{
490
    if (DistancePlaneToPoint(rclPoint) > fDistance) {
491
        return false;
492
    }
493

494
    // force internal normal to be computed if not done yet
495
    Base::Vector3f clNorm(GetNormal()), clProjPt(rclPoint), clEdge;
496
    Base::Vector3f clP0(_aclPoints[0]), clP1(_aclPoints[1]), clP2(_aclPoints[2]);
497
    float fLP {}, fLE {};
498

499
    clNorm.Normalize();
500
    clProjPt.ProjectToPlane(_aclPoints[0], clNorm);
501

502

503
    // Edge P0 --> P1
504
    clEdge = clP1 - clP0;
505
    fLP = clProjPt.DistanceToLine(clP0, clEdge);
506
    if (fLP > 0.0f) {
507
        fLE = clP2.DistanceToLine(clP0, clEdge);
508
        if (fLP <= fLE) {
509
            if (clProjPt.DistanceToLine(clP2, clEdge) > fLE) {
510
                return false;
511
            }
512
        }
513
        else {
514
            return false;
515
        }
516
    }
517

518
    // Edge P0 --> P2
519
    clEdge = clP2 - clP0;
520
    fLP = clProjPt.DistanceToLine(clP0, clEdge);
521
    if (fLP > 0.0f) {
522
        fLE = clP1.DistanceToLine(clP0, clEdge);
523
        if (fLP <= fLE) {
524
            if (clProjPt.DistanceToLine(clP1, clEdge) > fLE) {
525
                return false;
526
            }
527
        }
528
        else {
529
            return false;
530
        }
531
    }
532

533
    // Edge P1 --> P2
534
    clEdge = clP2 - clP1;
535
    fLP = clProjPt.DistanceToLine(clP1, clEdge);
536
    if (fLP > 0.0f) {
537
        fLE = clP0.DistanceToLine(clP1, clEdge);
538
        if (fLP <= fLE) {
539
            if (clProjPt.DistanceToLine(clP0, clEdge) > fLE) {
540
                return false;
541
            }
542
        }
543
        else {
544
            return false;
545
        }
546
    }
547

548
    return true;
549
}
550

551
bool MeshGeomFacet::IsPointOfFace(const Base::Vector3f& rclP, float fDistance) const
552
{
553
    // more effective implementation than in MeshGeomFacet::IsPointOf
554
    //
555
    Base::Vector3f a(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
556
    Base::Vector3f b(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
557
    Base::Vector3f c(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
558
    Base::Vector3f p(rclP);
559

560
    Base::Vector3f n = (b - a) % (c - a);
561
    Base::Vector3f n1 = (a - p) % (b - p);
562
    Base::Vector3f n2 = (c - p) % (a - p);
563
    Base::Vector3f n3 = (b - p) % (c - p);
564

565
    if (n * (p - a) > fDistance * n.Length()) {
566
        return false;
567
    }
568

569
    if (n * (a - p) > fDistance * n.Length()) {
570
        return false;
571
    }
572

573
    if (n * n1 <= 0.0f) {
574
        return false;
575
    }
576

577
    if (n * n2 <= 0.0f) {
578
        return false;
579
    }
580

581
    if (n * n3 <= 0.0f) {
582
        return false;
583
    }
584

585
    return true;
586
}
587

588
bool MeshGeomFacet::Weights(const Base::Vector3f& rclP, float& w0, float& w1, float& w2) const
589
{
590
    float fAreaABC = Area();
591
    float fAreaPBC = MeshGeomFacet(rclP, _aclPoints[1], _aclPoints[2]).Area();
592
    float fAreaPCA = MeshGeomFacet(rclP, _aclPoints[2], _aclPoints[0]).Area();
593
    float fAreaPAB = MeshGeomFacet(rclP, _aclPoints[0], _aclPoints[1]).Area();
594

595
    w0 = fAreaPBC / fAreaABC;
596
    w1 = fAreaPCA / fAreaABC;
597
    w2 = fAreaPAB / fAreaABC;
598

599
    return fabs(w0 + w1 + w2 - 1.0f) < 0.001f;
600
}
601

602
void MeshGeomFacet::ProjectPointToPlane(const Base::Vector3f& rclPoint,
603
                                        Base::Vector3f& rclProj) const
604
{
605
    rclPoint.ProjectToPlane(_aclPoints[0], GetNormal(), rclProj);
606
}
607

608
void MeshGeomFacet::ProjectFacetToPlane(MeshGeomFacet& rclFacet) const
609
{
610
    // project facet 2 onto facet 1
611
    IntersectPlaneWithLine(rclFacet._aclPoints[0], GetNormal(), rclFacet._aclPoints[0]);
612
    IntersectPlaneWithLine(rclFacet._aclPoints[1], GetNormal(), rclFacet._aclPoints[1]);
613
    IntersectPlaneWithLine(rclFacet._aclPoints[2], GetNormal(), rclFacet._aclPoints[2]);
614
}
615

616
void MeshGeomFacet::Enlarge(float fDist)
617
{
618
    Base::Vector3f clM, clU, clV, clPNew[3];
619
    float fA {}, fD {};
620
    PointIndex i {}, ulP1 {}, ulP2 {}, ulP3 {};
621

622
    for (i = 0; i < 3; i++) {
623
        ulP1 = i;
624
        ulP2 = (i + 1) % 3;
625
        ulP3 = (i + 2) % 3;
626
        clU = _aclPoints[ulP2] - _aclPoints[ulP1];
627
        clV = _aclPoints[ulP3] - _aclPoints[ulP1];
628
        clM = -(clU + clV);
629
        fA = clM.GetAngle(-clU);
630
        fD = fDist / float(sin(fA));
631
        clM.Normalize();
632
        clM.Scale(fD, fD, fD);
633
        clPNew[ulP1] = _aclPoints[ulP1] + clM;
634
    }
635

636
    _aclPoints[0] = clPNew[0];
637
    _aclPoints[1] = clPNew[1];
638
    _aclPoints[2] = clPNew[2];
639
}
640

641
bool MeshGeomFacet::IsDegenerated(float epsilon) const
642
{
643
    // The triangle has the points A,B,C where we can define the vector u and v
644
    // u = b-a and v = c-a. Then we define the line g: r = a+t*u and the plane
645
    // E: (x-c)*u=0. The intersection point of g and E is S.
646
    // The vector to S can be computed with a+(uv)/(uu)*u. The difference of
647
    // C and S then is v-(u*v)/(u*u)*u. The square distance leads to the formula
648
    // (v-(u*v)/(u*u)*u)^2 < eps which means that C and S is considered equal if
649
    // the square distance is less than an epsilon and thus the triangle is de-
650
    // generated.
651
    // After a few calculation step we get the formula:
652
    // (u*u)*(v*v)-(u*v)*(u*v) < eps*(u*u)
653
    // So, if we do the same except that we define a line h which goes through
654
    // A and C and a plane going through B we get a similar formula:
655
    // (u*u)*(v*v)-(u*v)*(u*v) < eps*(v*v).
656
    // As end formula we can write then:
657
    // (u*u)*(v*v)-(u*v)*(u*v) < max(eps*(u*u),eps*(v*v)).
658
    //
659
    // BTW (u*u)*(v*v)-(u*v)*(u*v) is the same as (uxv)*(uxv).
660
    Base::Vector3d p1 = Base::convertTo<Base::Vector3d>(this->_aclPoints[0]);
661
    Base::Vector3d p2 = Base::convertTo<Base::Vector3d>(this->_aclPoints[1]);
662
    Base::Vector3d p3 = Base::convertTo<Base::Vector3d>(this->_aclPoints[2]);
663

664
    Base::Vector3d u = p2 - p1;
665
    Base::Vector3d v = p3 - p1;
666

667
    double eps = static_cast<double>(epsilon);
668
    double uu = u * u;
669
    if (uu <= eps) {
670
        return true;
671
    }
672
    double vv = v * v;
673
    if (vv <= eps) {
674
        return true;
675
    }
676
    double uv = u * v;
677
    double crosssqr = uu * vv - uv * uv;
678
    if (crosssqr <= eps * std::max<double>(uu, vv)) {
679
        return true;
680
    }
681
    return false;
682
}
683

684
bool MeshGeomFacet::IsDeformed(float fCosOfMinAngle, float fCosOfMaxAngle) const
685
{
686
    float fCosAngle {};
687
    Base::Vector3f u, v;
688

689
    for (int i = 0; i < 3; i++) {
690
        u = _aclPoints[(i + 1) % 3] - _aclPoints[i];
691
        v = _aclPoints[(i + 2) % 3] - _aclPoints[i];
692
        u.Normalize();
693
        v.Normalize();
694

695
        fCosAngle = u * v;
696

697
        if (fCosAngle > fCosOfMinAngle || fCosAngle < fCosOfMaxAngle) {
698
            return true;
699
        }
700
    }
701

702
    return false;
703
}
704

705
bool MeshGeomFacet::IntersectBoundingBox(const Base::BoundBox3f& rclBB) const
706
{
707
    // the triangle's corner points
708
    const Base::Vector3f& v0 = _aclPoints[0];
709
    const Base::Vector3f& v1 = _aclPoints[1];
710
    const Base::Vector3f& v2 = _aclPoints[2];
711

712
    // first check if at least one point is inside the box
713
    if (rclBB.IsInBox(v0) || rclBB.IsInBox(v1) || rclBB.IsInBox(v2)) {
714
        return true;
715
    }
716

717
    // edge lengths
718
    float len0 = (v0 - v1).Length();
719
    float len1 = (v1 - v2).Length();
720
    float len2 = (v2 - v0).Length();
721

722
    // Build up the line segments
723
    Vector3<float> p0(0.5f * (v0.x + v1.x), 0.5f * (v0.y + v1.y), 0.5f * (v0.z + v1.z));
724
    Vector3<float> p1(0.5f * (v1.x + v2.x), 0.5f * (v1.y + v2.y), 0.5f * (v1.z + v2.z));
725
    Vector3<float> p2(0.5f * (v2.x + v0.x), 0.5f * (v2.y + v0.y), 0.5f * (v2.z + v0.z));
726

727
    Vector3<float> d0(v1.x - v0.x, v1.y - v0.y, v1.z - v0.z);
728
    d0.Normalize();
729
    Vector3<float> d1(v2.x - v1.x, v2.y - v1.y, v2.z - v1.z);
730
    d1.Normalize();
731
    Vector3<float> d2(v0.x - v2.x, v0.y - v2.y, v0.z - v2.z);
732
    d2.Normalize();
733

734
    Segment3<float> akSeg0(p0, d0, len0 / 2.0f);
735
    Segment3<float> akSeg1(p1, d1, len1 / 2.0f);
736
    Segment3<float> akSeg2(p2, d2, len2 / 2.0f);
737

738
    // Build up the box
739
    Base::Vector3f clCenter = rclBB.GetCenter();
740
    Vector3<float> center(clCenter.x, clCenter.y, clCenter.z);
741
    Vector3<float> axis0(1.0f, 0.0f, 0.0f);
742
    Vector3<float> axis1(0.0f, 1.0f, 0.0f);
743
    Vector3<float> axis2(0.0f, 0.0f, 1.0f);
744
    float extent0 = 0.5f * rclBB.LengthX();
745
    float extent1 = 0.5f * rclBB.LengthY();
746
    float extent2 = 0.5f * rclBB.LengthZ();
747

748
    Box3<float> akBox(center, axis0, axis1, axis2, extent0, extent1, extent2);
749

750
    // Check for intersection of line segments and box
751
    IntrSegment3Box3<float> akSec0(akSeg0, akBox, false);
752
    if (akSec0.Test()) {
753
        return true;
754
    }
755
    IntrSegment3Box3<float> akSec1(akSeg1, akBox, false);
756
    if (akSec1.Test()) {
757
        return true;
758
    }
759
    IntrSegment3Box3<float> akSec2(akSeg2, akBox, false);
760
    if (akSec2.Test()) {
761
        return true;
762
    }
763

764
    // no intersection
765
    return false;
766
}
767

768
bool MeshGeomFacet::IntersectWithPlane(const Base::Vector3f& rclBase,
769
                                       const Base::Vector3f& rclNormal,
770
                                       Base::Vector3f& rclP1,
771
                                       Base::Vector3f& rclP2) const
772
{
773
    const float eps = 1e-06f;
774

775
    // the triangle's corner points
776
    const Base::Vector3f& v0 = _aclPoints[0];
777
    const Base::Vector3f& v1 = _aclPoints[1];
778
    const Base::Vector3f& v2 = _aclPoints[2];
779

780
    // first check if a triangle's edge lies on the plane
781
    float dist0 = fabs(v0.DistanceToPlane(rclBase, rclNormal));
782
    float dist1 = fabs(v1.DistanceToPlane(rclBase, rclNormal));
783
    float dist2 = fabs(v2.DistanceToPlane(rclBase, rclNormal));
784
    if (dist0 < eps && dist1 < eps) {
785
        rclP1 = v0;
786
        rclP2 = v1;
787
        return true;
788
    }
789
    if (dist1 < eps && dist2 < eps) {
790
        rclP1 = v1;
791
        rclP2 = v2;
792
        return true;
793
    }
794
    if (dist2 < eps && dist0 < eps) {
795
        rclP1 = v2;
796
        rclP2 = v0;
797
        return true;
798
    }
799

800
    // edge lengths
801
    float len0 = (v0 - v1).Length();
802
    float len1 = (v1 - v2).Length();
803
    float len2 = (v2 - v0).Length();
804

805
    // Build up the line segments
806
    Vector3<float> p0(0.5f * (v0.x + v1.x), 0.5f * (v0.y + v1.y), 0.5f * (v0.z + v1.z));
807
    Vector3<float> p1(0.5f * (v1.x + v2.x), 0.5f * (v1.y + v2.y), 0.5f * (v1.z + v2.z));
808
    Vector3<float> p2(0.5f * (v2.x + v0.x), 0.5f * (v2.y + v0.y), 0.5f * (v2.z + v0.z));
809

810
    Vector3<float> d0(v1.x - v0.x, v1.y - v0.y, v1.z - v0.z);
811
    d0.Normalize();
812
    Vector3<float> d1(v2.x - v1.x, v2.y - v1.y, v2.z - v1.z);
813
    d1.Normalize();
814
    Vector3<float> d2(v0.x - v2.x, v0.y - v2.y, v0.z - v2.z);
815
    d2.Normalize();
816

817
    Segment3<float> akSeg0(p0, d0, len0 / 2.0f);
818
    Segment3<float> akSeg1(p1, d1, len1 / 2.0f);
819
    Segment3<float> akSeg2(p2, d2, len2 / 2.0f);
820

821
    // Build up the plane
822
    Vector3<float> p(rclBase.x, rclBase.y, rclBase.z);
823
    Vector3<float> n(rclNormal.x, rclNormal.y, rclNormal.z);
824
    Plane3<float> akPln(n, p);
825

826
    // Check for intersection with plane for each line segment
827
    IntrSegment3Plane3<float> test0(akSeg0, akPln);
828
    IntrSegment3Plane3<float> test1(akSeg1, akPln);
829
    IntrSegment3Plane3<float> test2(akSeg2, akPln);
830

831
    Vector3<float> intr;
832

833
    // now check if a triangle's corner lies on the plane
834
    if (dist0 < eps) {
835
        rclP1 = v0;
836
        rclP2 = v0;
837
        if (test1.Find()) {
838
            intr = p1 + test1.GetSegmentT() * d1;
839
            rclP2.Set(intr[0], intr[1], intr[2]);
840
        }
841
        return true;
842
    }
843
    else if (dist1 < eps) {
844
        rclP1 = v1;
845
        rclP2 = v1;
846
        if (test2.Find()) {
847
            intr = p2 + test2.GetSegmentT() * d2;
848
            rclP2.Set(intr[0], intr[1], intr[2]);
849
        }
850
        return true;
851
    }
852
    else if (dist2 < eps) {
853
        rclP1 = v2;
854
        rclP2 = v2;
855
        if (test0.Find()) {
856
            intr = p0 + test0.GetSegmentT() * d0;
857
            rclP2.Set(intr[0], intr[1], intr[2]);
858
        }
859
        return true;
860
    }
861

862
    // check for arbitrary intersections
863
    if (test0.Find()) {
864
        intr = p0 + test0.GetSegmentT() * d0;
865
        rclP1.Set(intr[0], intr[1], intr[2]);
866

867
        if (test1.Find()) {
868
            intr = p1 + test1.GetSegmentT() * d1;
869
            rclP2.Set(intr[0], intr[1], intr[2]);
870
            return true;
871
        }
872
        else if (test2.Find()) {
873
            intr = p2 + test2.GetSegmentT() * d2;
874
            rclP2.Set(intr[0], intr[1], intr[2]);
875
            return true;
876
        }
877
    }
878
    else if (test1.Find()) {
879
        intr = p1 + test1.GetSegmentT() * d1;
880
        rclP1.Set(intr[0], intr[1], intr[2]);
881

882
        if (test2.Find()) {
883
            intr = p2 + test2.GetSegmentT() * d2;
884
            rclP2.Set(intr[0], intr[1], intr[2]);
885
            return true;
886
        }
887
    }
888

889
    return false;
890
}
891

892
bool MeshGeomFacet::Foraminate(const Base::Vector3f& P,
893
                               const Base::Vector3f& dir,
894
                               Base::Vector3f& I,
895
                               float fMaxAngle) const
896
{
897
    const float eps = 1e-06f;
898
    Base::Vector3f n = this->GetNormal();
899

900
    // check angle between facet normal and the line direction, FLOAT_MAX is
901
    // returned for degenerated facets
902
    float fAngle = dir.GetAngle(n);
903
    if (fAngle > fMaxAngle) {
904
        return false;
905
    }
906

907
    float nn = n * n;
908
    float nd = n * dir;
909
    float dd = dir * dir;
910

911
    // the line mustn't be parallel to the triangle
912
    if ((nd * nd) <= (eps * dd * nn)) {
913
        return false;
914
    }
915

916
    Base::Vector3f u = this->_aclPoints[1] - this->_aclPoints[0];
917
    Base::Vector3f v = this->_aclPoints[2] - this->_aclPoints[0];
918

919
    Base::Vector3f w0 = P - this->_aclPoints[0];
920
    float r = -(n * w0) / nd;
921
    Base::Vector3f w = w0 + r * dir;
922

923
    float uu = u * u;
924
    float uv = u * v;
925
    float vv = v * v;
926
    float wu = w * u;
927
    float wv = w * v;
928
    float det = float(fabs((uu * vv) - (uv * uv)));
929

930
    float s = (vv * wu) - (uv * wv);
931
    float t = (uu * wv) - (uv * wu);
932

933
    // is the intersection point inside the triangle?
934
    if ((s >= 0.0f) && (t >= 0.0f) && ((s + t) <= det)) {
935
        I = w + this->_aclPoints[0];
936
        return true;
937
    }
938

939
    return false;
940
}
941

942
bool MeshGeomFacet::IntersectPlaneWithLine(const Base::Vector3f& rclPt,
943
                                           const Base::Vector3f& rclDir,
944
                                           Base::Vector3f& rclRes) const
945
{
946
    // calculate the intersection of the straight line <-> plane
947
    if (fabs(rclDir * GetNormal()) < 1e-3f) {
948
        return false;  // line and plane are parallel
949
    }
950

951
    float s = ((GetGravityPoint() - rclPt) * GetNormal()) / (rclDir * GetNormal());
952
    rclRes = rclPt + s * rclDir;
953

954
    return true;
955
}
956

957
bool MeshGeomFacet::IntersectWithLine(const Base::Vector3f& rclPt,
958
                                      const Base::Vector3f& rclDir,
959
                                      Base::Vector3f& rclRes) const
960
{
961
    if (!IntersectPlaneWithLine(rclPt, rclDir, rclRes)) {
962
        return false;  // line and plane are parallel
963
    }
964
    // Check if the intersection point is inside the facet
965
    return IsPointOfFace(rclRes, 1e-03f);
966
}
967

968
float MeshGeomFacet::DistanceToLineSegment(const Base::Vector3f& rclP1,
969
                                           const Base::Vector3f& rclP2) const
970
{
971
    // line segment
972
    Vector3<float> A(rclP1.x, rclP1.y, rclP1.z);
973
    Vector3<float> B(rclP2.x, rclP2.y, rclP2.z);
974

975
    Vector3<float> n = B - A;
976
    float len = n.Length();
977
    n.Normalize();
978
    Vector3<float> p = 0.5f * (A + B);
979

980
    Segment3<float> akSeg(p, n, 0.5f * len);
981

982
    // triangle
983
    Vector3<float> akF0(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
984
    Vector3<float> akF1(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
985
    Vector3<float> akF2(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
986

987
    Triangle3<float> akTria(akF0, akF1, akF2);
988

989
    DistSegment3Triangle3<float> akDistSegTria(akSeg, akTria);
990
    return akDistSegTria.Get();
991
}
992

993
float MeshGeomFacet::DistanceToPoint(const Base::Vector3f& rclPt, Base::Vector3f& rclNt) const
994
{
995
    Vector3<float> akPt(rclPt.x, rclPt.y, rclPt.z);
996
    Vector3<float> akF0(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
997
    Vector3<float> akF1(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
998
    Vector3<float> akF2(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
999

1000
    Triangle3<float> akTria(akF0, akF1, akF2);
1001
    DistVector3Triangle3<float> akDistPtTria(akPt, akTria);
1002

1003
    float fDist = akDistPtTria.Get();
1004

1005
    // get nearest point of the facet
1006
    Vector3<float> akNt = akDistPtTria.GetClosestPoint1();
1007
    rclNt.Set(akNt.X(), akNt.Y(), akNt.Z());
1008

1009
    return fDist;
1010
}
1011

1012
void MeshGeomFacet::SubSample(float fStep, std::vector<Base::Vector3f>& rclPoints) const
1013
{
1014
    std::vector<Base::Vector3f> clPoints;
1015
    Base::Vector3f A = _aclPoints[0];
1016
    Base::Vector3f B = _aclPoints[1];
1017
    Base::Vector3f C = _aclPoints[2];
1018
    Base::Vector3f clVecAB(B - A);
1019
    Base::Vector3f clVecAC(C - A);
1020
    Base::Vector3f clVecBC(C - B);
1021

1022
    // longest axis corresponds to AB
1023
    float fLenAB = clVecAB.Length();
1024
    float fLenAC = clVecAC.Length();
1025
    float fLenBC = clVecBC.Length();
1026

1027
    if (fLenAC > fLenAB) {
1028
        std::swap(B, C);
1029
        std::swap(fLenAB, fLenAC);
1030
    }
1031
    if (fLenBC > fLenAB) {
1032
        std::swap(A, C);
1033
        std::swap(fLenBC, fLenAB);
1034
    }
1035

1036
    clVecAB = (B - A);
1037
    clVecAC = (C - A);
1038
    clVecBC = (C - B);
1039
    Base::Vector3f clVecABNorm(clVecAB);
1040
    Base::Vector3f clVecHNorm((clVecAB % clVecAC) % clVecAB);
1041
    clVecABNorm.Normalize();
1042
    clVecHNorm.Normalize();
1043

1044
    float bx = fLenAB;
1045
    float cy = float(sin(clVecAB.GetAngle(clVecAC)) * fLenAC);
1046
    float cx = float(sqrt(fabs(fLenAC * fLenAC - cy * cy)));
1047

1048
    float fDetABC = bx * cy;
1049

1050
    for (float px = (fStep / 2.0f); px < fLenAB; px += fStep)  // NOLINT
1051
    {
1052
        for (float py = (fStep / 2.0f); py < cy; py += fStep)  // NOLINT
1053
        {
1054
            float u = (bx * cy + cx * py - px * cy - bx * py) / fDetABC;
1055
            float v = (px * cy - cx * py) / fDetABC;
1056
            float w = (bx * py) / fDetABC;
1057

1058
            if ((u >= 0.0f) && (v >= 0.0f) && (w >= 0.0f) && ((u + v) < 1.0f)) {
1059
                // rclPoints.push_back(CBase::Vector3f(u*A + v*B + w*C));
1060
                Base::Vector3f clV = A + (px * clVecABNorm) + (py * clVecHNorm);
1061
                clPoints.push_back(clV);
1062
            }
1063
            else {
1064
                break;
1065
            }
1066
        }
1067
    }
1068

1069
    // if couldn't subsample the facet take gravity center
1070
    if (clPoints.empty()) {
1071
        clPoints.push_back(this->GetGravityPoint());
1072
    }
1073

1074
    rclPoints.insert(rclPoints.end(), clPoints.begin(), clPoints.end());
1075
}
1076

1077
bool MeshGeomFacet::IsCoplanar(const MeshGeomFacet& facet) const
1078
{
1079
    const float eps = 1e-06f;
1080
    const float unit = 0.9995f;
1081
    float mult = fabs(this->GetNormal() * facet.GetNormal());
1082
    float dist = fabs(DistancePlaneToPoint(facet._aclPoints[0]));
1083
    return (mult >= unit) && (dist <= eps);
1084
}
1085

1086
/**
1087
 * Fast Triangle-Triangle Intersection Test by Tomas Moeller
1088
 * http://www.acm.org/jgt/papers/Moller97/tritri.html
1089
 * http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/
1090
 */
1091
bool MeshGeomFacet::IntersectWithFacet(const MeshGeomFacet& rclFacet) const
1092
{
1093
    float V[3][3], U[3][3];
1094
    for (int i = 0; i < 3; i++) {
1095
        V[i][0] = _aclPoints[i].x;
1096
        V[i][1] = _aclPoints[i].y;
1097
        V[i][2] = _aclPoints[i].z;
1098
        U[i][0] = rclFacet._aclPoints[i].x;
1099
        U[i][1] = rclFacet._aclPoints[i].y;
1100
        U[i][2] = rclFacet._aclPoints[i].z;
1101
    }
1102

1103
    if (tri_tri_intersect(V[0], V[1], V[2], U[0], U[1], U[2]) == 0) {
1104
        return false;  // no intersections
1105
    }
1106
    return true;
1107
}
1108

1109
/**
1110
 * Fast Triangle-Triangle Intersection Test by Tomas Moeller
1111
 * http://www.acm.org/jgt/papers/Moller97/tritri.html
1112
 * http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/
1113
 */
1114
int MeshGeomFacet::IntersectWithFacet(const MeshGeomFacet& rclFacet,
1115
                                      Base::Vector3f& rclPt0,
1116
                                      Base::Vector3f& rclPt1) const
1117
{
1118
    // Note: tri_tri_intersect_with_isection() does not return line of
1119
    // intersection when triangles are coplanar. See tritritest.h:18 and 658.
1120
    if (IsCoplanar(rclFacet)) {
1121
        // Since tri_tri_intersect_with_isection may return garbage values try to get
1122
        // sensible values with edge/edge intersections
1123
        std::vector<Base::Vector3f> intersections;
1124
        for (short i = 0; i < 3; i++) {
1125
            MeshGeomEdge edge1 = GetEdge(i);
1126
            for (short j = 0; j < 3; j++) {
1127
                MeshGeomEdge edge2 = rclFacet.GetEdge(j);
1128
                Base::Vector3f point;
1129
                if (edge1.IntersectWithEdge(edge2, point)) {
1130
                    intersections.push_back(point);
1131
                }
1132
            }
1133
        }
1134

1135
        // If triangles overlap there can be more than two intersection points
1136
        // In that case use any two of them.
1137
        if (intersections.size() >= 2) {
1138
            rclPt0 = intersections[0];
1139
            rclPt1 = intersections[1];
1140
            return 2;
1141
        }
1142
        else if (intersections.size() == 1) {
1143
            rclPt0 = intersections[0];
1144
            rclPt1 = intersections[0];
1145
            return 1;
1146
        }
1147

1148
        return 0;
1149
    }
1150

1151
    float V[3][3] {}, U[3][3] {};
1152
    int coplanar = 0;
1153
    float isectpt1[3] {}, isectpt2[3] {};
1154

1155
    for (int i = 0; i < 3; i++) {
1156
        V[i][0] = _aclPoints[i].x;
1157
        V[i][1] = _aclPoints[i].y;
1158
        V[i][2] = _aclPoints[i].z;
1159
        U[i][0] = rclFacet._aclPoints[i].x;
1160
        U[i][1] = rclFacet._aclPoints[i].y;
1161
        U[i][2] = rclFacet._aclPoints[i].z;
1162
    }
1163

1164
    if (tri_tri_intersect_with_isectline(V[0],
1165
                                         V[1],
1166
                                         V[2],
1167
                                         U[0],
1168
                                         U[1],
1169
                                         U[2],
1170
                                         &coplanar,
1171
                                         isectpt1,
1172
                                         isectpt2)
1173
        == 0) {
1174
        return 0;  // no intersections
1175
    }
1176

1177
    rclPt0.x = isectpt1[0];
1178
    rclPt0.y = isectpt1[1];
1179
    rclPt0.z = isectpt1[2];
1180
    rclPt1.x = isectpt2[0];
1181
    rclPt1.y = isectpt2[1];
1182
    rclPt1.z = isectpt2[2];
1183

1184
    // With extremely acute-angled triangles it may happen that the algorithm
1185
    // claims an intersection but the intersection points are far outside the
1186
    // model. So, a plausibility check is to verify that the intersection points
1187
    // are inside the bounding boxes of both triangles.
1188
    Base::BoundBox3f box1 = this->GetBoundBox();
1189
    box1.Enlarge(0.001f);
1190
    if (!box1.IsInBox(rclPt0) || !box1.IsInBox(rclPt1)) {
1191
        return 0;
1192
    }
1193

1194
    Base::BoundBox3f box2 = rclFacet.GetBoundBox();
1195
    box2.Enlarge(0.001f);
1196
    if (!box2.IsInBox(rclPt0) || !box2.IsInBox(rclPt1)) {
1197
        return 0;
1198
    }
1199

1200
    // Note: The algorithm delivers sometimes false-positives, i.e. it claims
1201
    // that the two triangles intersect but they don't. It seems that this bad
1202
    // behaviour occurs if the triangles are nearly co-planar
1203
    float mult = fabs(this->GetNormal() * rclFacet.GetNormal());
1204
    if (rclPt0 == rclPt1) {
1205
        if (mult < 0.995f) {  // not co-planar, thus no test needed
1206
            return 1;
1207
        }
1208
        if (this->IsPointOf(rclPt0) && rclFacet.IsPointOf(rclPt0)) {
1209
            return 1;
1210
        }
1211
    }
1212
    else {
1213
        if (mult < 0.995f) {  // not co-planar, thus no test needed
1214
            return 2;
1215
        }
1216
        if (this->IsPointOf(rclPt0) && rclFacet.IsPointOf(rclPt0) && this->IsPointOf(rclPt1)
1217
            && rclFacet.IsPointOf(rclPt1)) {
1218
            return 2;
1219
        }
1220
    }
1221

1222
    // the intersection algorithm delivered a false-positive
1223
    return 0;
1224
}
1225

1226
bool MeshGeomFacet::IsPointOf(const Base::Vector3f& P) const
1227
{
1228
    Base::Vector3d p1 = Base::convertTo<Base::Vector3d>(this->_aclPoints[0]);
1229
    Base::Vector3d p2 = Base::convertTo<Base::Vector3d>(this->_aclPoints[1]);
1230
    Base::Vector3d p3 = Base::convertTo<Base::Vector3d>(this->_aclPoints[2]);
1231
    Base::Vector3d p4 = Base::convertTo<Base::Vector3d>(P);
1232

1233
    Base::Vector3d u = p2 - p1;
1234
    Base::Vector3d v = p3 - p1;
1235
    Base::Vector3d w = p4 - p1;
1236

1237
    double uu = u * u;
1238
    double uv = u * v;
1239
    double vv = v * v;
1240
    double wu = w * u;
1241
    double wv = w * v;
1242
    double det = fabs((uu * vv) - (uv * uv));
1243

1244
    // Note: Due to roundoff errors it can happen that we get very small
1245
    // negative values for s or t. This e.g. can happen if the point lies
1246
    // at the border of the facet. And as det could also become very small
1247
    // we need an adaptive tolerance.
1248
    const double eps = std::min<double>(1.0e-6, det * det);
1249

1250
    double s = (vv * wu) - (uv * wv);
1251
    double t = (uu * wv) - (uv * wu);
1252

1253
    // is the point inside the triangle?
1254
    if ((s >= -eps) && (t >= -eps) && ((s + t) <= det + eps)) {
1255
        return true;
1256
    }
1257

1258
    return false;
1259
}
1260

1261
float MeshGeomFacet::CenterOfInscribedCircle(Base::Vector3f& rclCenter) const
1262
{
1263
    const Base::Vector3f& p0 = _aclPoints[0];
1264
    const Base::Vector3f& p1 = _aclPoints[1];
1265
    const Base::Vector3f& p2 = _aclPoints[2];
1266

1267
    float a = Base::Distance(p1, p2);
1268
    float b = Base::Distance(p2, p0);
1269
    float c = Base::Distance(p0, p1);
1270

1271
    // radius of the circle
1272
    float fRadius = Area();
1273
    fRadius *= 2.0f / (a + b + c);
1274

1275
    // center of the circle
1276
    float w = a + b + c;
1277
    rclCenter.x = (a * p0.x + b * p1.x + c * p2.x) / w;
1278
    rclCenter.y = (a * p0.y + b * p1.y + c * p2.y) / w;
1279
    rclCenter.z = (a * p0.z + b * p1.z + c * p2.z) / w;
1280

1281
    return fRadius;
1282
}
1283

1284
float MeshGeomFacet::CenterOfCircumCircle(Base::Vector3f& rclCenter) const
1285
{
1286
    const Base::Vector3f& p0 = _aclPoints[0];
1287
    const Base::Vector3f& p1 = _aclPoints[1];
1288
    const Base::Vector3f& p2 = _aclPoints[2];
1289

1290
    Base::Vector3f u = (p1 - p0);
1291
    Base::Vector3f v = (p2 - p1);
1292
    Base::Vector3f w = (p0 - p2);
1293

1294
    double uu = (u * u);
1295
    double vv = (v * v);
1296
    double ww = (w * w);
1297
    double uv = -(u * v);
1298
    double vw = -(v * w);
1299
    double uw = -(w * u);
1300

1301
    double w0 = (2 * sqrt(uu * ww - uw * uw) * uw / (uu * ww));
1302
    double w1 = (2 * sqrt(uu * vv - uv * uv) * uv / (uu * vv));
1303
    double w2 = (2 * sqrt(vv * ww - vw * vw) * vw / (vv * ww));
1304

1305
    // center of the circle
1306
    double wx = w0 + w1 + w2;
1307
    rclCenter.x = static_cast<float>((w0 * p0.x + w1 * p1.x + w2 * p2.x) / wx);
1308
    rclCenter.y = static_cast<float>((w0 * p0.y + w1 * p1.y + w2 * p2.y) / wx);
1309
    rclCenter.z = static_cast<float>((w0 * p0.z + w1 * p1.z + w2 * p2.z) / wx);
1310

1311
    // radius of the circle
1312
    float fRadius = static_cast<float>(sqrt(uu * vv * ww) / (4 * Area()));
1313
    return fRadius;
1314
}
1315

1316
unsigned short MeshGeomFacet::NearestEdgeToPoint(const Base::Vector3f& rclPt) const
1317
{
1318
    unsigned short usSide {};
1319

1320
    const Base::Vector3f& rcP1 = _aclPoints[0];
1321
    const Base::Vector3f& rcP2 = _aclPoints[1];
1322
    const Base::Vector3f& rcP3 = _aclPoints[2];
1323

1324
    float fD1 = FLOAT_MAX;
1325
    float fD2 = FLOAT_MAX;
1326
    float fD3 = FLOAT_MAX;
1327

1328
    // 1st edge
1329
    Base::Vector3f clDir = rcP2 - rcP1;
1330
    float fLen = Base::Distance(rcP2, rcP1);
1331
    float t = ((rclPt - rcP1) * clDir) / (fLen * fLen);
1332
    if (t < 0.0f) {
1333
        fD1 = Base::Distance(rclPt, rcP1);
1334
    }
1335
    else if (t > 1.0f) {
1336
        fD1 = Base::Distance(rclPt, rcP2);
1337
    }
1338
    else {
1339
        fD1 = (((rclPt - rcP1) % clDir).Length()) / fLen;
1340
    }
1341

1342
    // 2nd edge
1343
    clDir = rcP3 - rcP2;
1344
    fLen = Base::Distance(rcP3, rcP2);
1345
    t = ((rclPt - rcP2) * clDir) / (fLen * fLen);
1346
    if (t < 0.0f) {
1347
        fD2 = Base::Distance(rclPt, rcP2);
1348
    }
1349
    else if (t > 1.0f) {
1350
        fD2 = Base::Distance(rclPt, rcP3);
1351
    }
1352
    else {
1353
        fD2 = (((rclPt - rcP2) % clDir).Length()) / fLen;
1354
    }
1355

1356
    // 3rd edge
1357
    clDir = rcP1 - rcP3;
1358
    fLen = Base::Distance(rcP1, rcP3);
1359
    t = ((rclPt - rcP3) * clDir) / (fLen * fLen);
1360
    if (t < 0.0f) {
1361
        fD3 = Base::Distance(rclPt, rcP3);
1362
    }
1363
    else if (t > 1.0f) {
1364
        fD3 = Base::Distance(rclPt, rcP1);
1365
    }
1366
    else {
1367
        fD3 = (((rclPt - rcP3) % clDir).Length()) / fLen;
1368
    }
1369

1370
    if (fD1 < fD2) {
1371
        if (fD1 < fD3) {
1372
            usSide = 0;
1373
        }
1374
        else {
1375
            usSide = 2;
1376
        }
1377
    }
1378
    else {
1379
        if (fD2 < fD3) {
1380
            usSide = 1;
1381
        }
1382
        else {
1383
            usSide = 2;
1384
        }
1385
    }
1386

1387
    return usSide;
1388
}
1389

1390
void MeshGeomFacet::NearestEdgeToPoint(const Base::Vector3f& rclPt,
1391
                                       float& fDistance,
1392
                                       unsigned short& usSide) const
1393
{
1394
    const Base::Vector3f& rcP1 = _aclPoints[0];
1395
    const Base::Vector3f& rcP2 = _aclPoints[1];
1396
    const Base::Vector3f& rcP3 = _aclPoints[2];
1397

1398
    float fD1 = FLOAT_MAX;
1399
    float fD2 = FLOAT_MAX;
1400
    float fD3 = FLOAT_MAX;
1401

1402
    // 1st edge
1403
    Base::Vector3f clDir = rcP2 - rcP1;
1404
    float fLen = Base::Distance(rcP2, rcP1);
1405
    float t = ((rclPt - rcP1) * clDir) / (fLen * fLen);
1406
    if (t < 0.0f) {
1407
        fD1 = Base::Distance(rclPt, rcP1);
1408
    }
1409
    else if (t > 1.0f) {
1410
        fD1 = Base::Distance(rclPt, rcP2);
1411
    }
1412
    else {
1413
        fD1 = (((rclPt - rcP1) % clDir).Length()) / fLen;
1414
    }
1415

1416
    // 2nd edge
1417
    clDir = rcP3 - rcP2;
1418
    fLen = Base::Distance(rcP3, rcP2);
1419
    t = ((rclPt - rcP2) * clDir) / (fLen * fLen);
1420
    if (t < 0.0f) {
1421
        fD2 = Base::Distance(rclPt, rcP2);
1422
    }
1423
    else if (t > 1.0f) {
1424
        fD2 = Base::Distance(rclPt, rcP3);
1425
    }
1426
    else {
1427
        fD2 = (((rclPt - rcP2) % clDir).Length()) / fLen;
1428
    }
1429

1430
    // 3rd edge
1431
    clDir = rcP1 - rcP3;
1432
    fLen = Base::Distance(rcP1, rcP3);
1433
    t = ((rclPt - rcP3) * clDir) / (fLen * fLen);
1434
    if (t < 0.0f) {
1435
        fD3 = Base::Distance(rclPt, rcP3);
1436
    }
1437
    else if (t > 1.0f) {
1438
        fD3 = Base::Distance(rclPt, rcP1);
1439
    }
1440
    else {
1441
        fD3 = (((rclPt - rcP3) % clDir).Length()) / fLen;
1442
    }
1443

1444
    if (fD1 < fD2) {
1445
        if (fD1 < fD3) {
1446
            usSide = 0;
1447
            fDistance = fD1;
1448
        }
1449
        else {
1450
            usSide = 2;
1451
            fDistance = fD3;
1452
        }
1453
    }
1454
    else {
1455
        if (fD2 < fD3) {
1456
            usSide = 1;
1457
            fDistance = fD2;
1458
        }
1459
        else {
1460
            usSide = 2;
1461
            fDistance = fD3;
1462
        }
1463
    }
1464
}
1465

1466
MeshGeomEdge MeshGeomFacet::GetEdge(short side) const
1467
{
1468
    MeshGeomEdge edge;
1469
    edge._aclPoints[0] = this->_aclPoints[side % 3];
1470
    edge._aclPoints[1] = this->_aclPoints[(side + 1) % 3];
1471
    return edge;
1472
}
1473

1474
float MeshGeomFacet::VolumeOfPrism(const MeshGeomFacet& rclF1) const
1475
{
1476
    Base::Vector3f P1 = this->_aclPoints[0];
1477
    Base::Vector3f P2 = this->_aclPoints[1];
1478
    Base::Vector3f P3 = this->_aclPoints[2];
1479
    Base::Vector3f Q1 = rclF1._aclPoints[0];
1480
    Base::Vector3f Q2 = rclF1._aclPoints[1];
1481
    Base::Vector3f Q3 = rclF1._aclPoints[2];
1482

1483
    if ((P1 - Q2).Length() < (P1 - Q1).Length()) {
1484
        Base::Vector3f tmp = Q1;
1485
        Q1 = Q2;
1486
        Q2 = tmp;
1487
    }
1488
    if ((P1 - Q3).Length() < (P1 - Q1).Length()) {
1489
        Base::Vector3f tmp = Q1;
1490
        Q1 = Q3;
1491
        Q3 = tmp;
1492
    }
1493
    if ((P2 - Q3).Length() < (P2 - Q2).Length()) {
1494
        Base::Vector3f tmp = Q2;
1495
        Q2 = Q3;
1496
        Q3 = tmp;
1497
    }
1498

1499
    Base::Vector3f N1 = (P2 - P1) % (P3 - P1);
1500
    Base::Vector3f N2 = (P2 - P1) % (Q2 - P1);
1501
    Base::Vector3f N3 = (Q2 - P1) % (Q1 - P1);
1502

1503
    float fVol = 0.0f;
1504
    fVol += float(fabs((Q3 - P1) * N1));
1505
    fVol += float(fabs((Q3 - P1) * N2));
1506
    fVol += float(fabs((Q3 - P1) * N3));
1507

1508
    fVol /= 6.0f;
1509

1510
    return fVol;
1511
    ;
1512
}
1513

1514
float MeshGeomFacet::MaximumAngle() const
1515
{
1516
    float fMaxAngle = 0.0f;
1517

1518
    for (int i = 0; i < 3; i++) {
1519
        Base::Vector3f dir1(_aclPoints[(i + 1) % 3] - _aclPoints[i]);
1520
        Base::Vector3f dir2(_aclPoints[(i + 2) % 3] - _aclPoints[i]);
1521
        float fAngle = dir1.GetAngle(dir2);
1522
        if (fAngle > fMaxAngle) {
1523
            fMaxAngle = fAngle;
1524
        }
1525
    }
1526

1527
    return fMaxAngle;
1528
}
1529

1530
float MeshGeomFacet::MinimumAngle() const
1531
{
1532
    float fMinAngle = Mathf::PI;
1533

1534
    for (int i = 0; i < 3; i++) {
1535
        Base::Vector3f dir1(_aclPoints[(i + 1) % 3] - _aclPoints[i]);
1536
        Base::Vector3f dir2(_aclPoints[(i + 2) % 3] - _aclPoints[i]);
1537
        float fAngle = dir1.GetAngle(dir2);
1538
        if (fAngle < fMinAngle) {
1539
            fMinAngle = fAngle;
1540
        }
1541
    }
1542

1543
    return fMinAngle;
1544
}
1545

1546
bool MeshGeomFacet::IsPointOfSphere(const Base::Vector3f& rP) const
1547
{
1548
    float radius {};
1549
    Base::Vector3f center;
1550
    radius = CenterOfCircumCircle(center);
1551
    radius *= radius;
1552

1553
    float dist = Base::DistanceP2(rP, center);
1554
    return dist < radius;
1555
}
1556

1557
bool MeshGeomFacet::IsPointOfSphere(const MeshGeomFacet& rFacet) const
1558
{
1559
    float radius {};
1560
    Base::Vector3f center;
1561
    radius = CenterOfCircumCircle(center);
1562
    radius *= radius;
1563

1564
    for (const auto& pnt : rFacet._aclPoints) {
1565
        float dist = Base::DistanceP2(pnt, center);
1566
        if (dist < radius) {
1567
            return true;
1568
        }
1569
    }
1570

1571
    return false;
1572
}
1573

1574
float MeshGeomFacet::AspectRatio() const
1575
{
1576
    Base::Vector3f d0 = _aclPoints[0] - _aclPoints[1];
1577
    Base::Vector3f d1 = _aclPoints[1] - _aclPoints[2];
1578
    Base::Vector3f d2 = _aclPoints[2] - _aclPoints[0];
1579

1580
    float l2 {}, maxl2 = d0.Sqr();
1581
    if ((l2 = d1.Sqr()) > maxl2) {
1582
        maxl2 = l2;
1583
    }
1584

1585
    d1 = d2;
1586
    if ((l2 = d1.Sqr()) > maxl2) {
1587
        maxl2 = l2;
1588
    }
1589

1590
    // squared area of the parallelogram spanned by d0 and d1
1591
    float a2 = (d0 % d1).Sqr();
1592
    return float(sqrt((maxl2 * maxl2) / a2));
1593
}
1594

1595
float MeshGeomFacet::AspectRatio2() const
1596
{
1597
    const Base::Vector3f& rcP1 = _aclPoints[0];
1598
    const Base::Vector3f& rcP2 = _aclPoints[1];
1599
    const Base::Vector3f& rcP3 = _aclPoints[2];
1600

1601
    float a = Base::Distance(rcP1, rcP2);
1602
    float b = Base::Distance(rcP2, rcP3);
1603
    float c = Base::Distance(rcP3, rcP1);
1604

1605
    // https://stackoverflow.com/questions/10289752/aspect-ratio-of-a-triangle-of-a-meshed-surface
1606
    return a * b * c / ((b + c - a) * (c + a - b) * (a + b - c));
1607
}
1608

1609
float MeshGeomFacet::Roundness() const
1610
{
1611
    const double FOUR_ROOT3 = 6.928203230275509;
1612
    double area = static_cast<double>(Area());
1613
    Base::Vector3f d0 = _aclPoints[0] - _aclPoints[1];
1614
    Base::Vector3f d1 = _aclPoints[1] - _aclPoints[2];
1615
    Base::Vector3f d2 = _aclPoints[2] - _aclPoints[0];
1616

1617
    double sum = static_cast<double>(d0.Sqr() + d1.Sqr() + d2.Sqr());
1618
    return static_cast<float>(FOUR_ROOT3 * area / sum);
1619
}
1620

1621
void MeshGeomFacet::Transform(const Base::Matrix4D& mat)
1622
{
1623
    mat.multVec(_aclPoints[0], _aclPoints[0]);
1624
    mat.multVec(_aclPoints[1], _aclPoints[1]);
1625
    mat.multVec(_aclPoints[2], _aclPoints[2]);
1626
    NormalInvalid();
1627
}
1628
FreeCAD

Использование cookies
