FreeCAD

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
#ifndef _PreComp_
26
#include <algorithm>
27
#endif
28

29
#include "Algorithm.h"
30
#include "Grid.h"
31
#include "Iterator.h"
32
#include "MeshKernel.h"
33

34

35
using namespace MeshCore;
36

37
MeshGrid::MeshGrid(const MeshKernel& rclM)
38
    : _pclMesh(&rclM)
39
    , _ulCtElements(0)
40
    , _ulCtGridsX(0)
41
    , _ulCtGridsY(0)
42
    , _ulCtGridsZ(0)
43
    , _fGridLenX(0.0f)
44
    , _fGridLenY(0.0f)
45
    , _fGridLenZ(0.0f)
46
    , _fMinX(0.0f)
47
    , _fMinY(0.0f)
48
    , _fMinZ(0.0f)
49
{}
50

51
MeshGrid::MeshGrid()
52
    : _pclMesh(nullptr)
53
    , _ulCtElements(0)
54
    , _ulCtGridsX(MESH_CT_GRID)
55
    , _ulCtGridsY(MESH_CT_GRID)
56
    , _ulCtGridsZ(MESH_CT_GRID)
57
    , _fGridLenX(0.0f)
58
    , _fGridLenY(0.0f)
59
    , _fGridLenZ(0.0f)
60
    , _fMinX(0.0f)
61
    , _fMinY(0.0f)
62
    , _fMinZ(0.0f)
63
{}
64

65
void MeshGrid::Attach(const MeshKernel& rclM)
66
{
67
    _pclMesh = &rclM;
68
    RebuildGrid();
69
}
70

71
void MeshGrid::Clear()
72
{
73
    _aulGrid.clear();
74
    _pclMesh = nullptr;
75
}
76

77
void MeshGrid::Rebuild(unsigned long ulX, unsigned long ulY, unsigned long ulZ)
78
{
79
    _ulCtGridsX = ulX;
80
    _ulCtGridsY = ulY;
81
    _ulCtGridsZ = ulZ;
82
    _ulCtElements = HasElements();
83
    RebuildGrid();
84
}
85

86
void MeshGrid::Rebuild(unsigned long ulPerGrid, unsigned long ulMaxGrid)
87
{
88
    _ulCtElements = HasElements();
89
    CalculateGridLength(ulPerGrid, ulMaxGrid);
90
    RebuildGrid();
91
}
92

93
void MeshGrid::Rebuild(int iCtGridPerAxis)
94
{
95
    _ulCtElements = HasElements();
96
    CalculateGridLength(iCtGridPerAxis);
97
    RebuildGrid();
98
}
99

100
void MeshGrid::InitGrid()
101
{
102
    assert(_pclMesh);
103

104
    // Calculate grid length if not initialised
105
    //
106
    if ((_ulCtGridsX == 0) || (_ulCtGridsY == 0) || (_ulCtGridsZ == 0)) {
107
        CalculateGridLength(MESH_CT_GRID, MESH_MAX_GRIDS);
108
    }
109

110
    // Determine grid length and offset
111
    //
112
    {
113
        Base::BoundBox3f clBBMesh = _pclMesh->GetBoundBox();
114

115
        float fLengthX = clBBMesh.LengthX();
116
        float fLengthY = clBBMesh.LengthY();
117
        float fLengthZ = clBBMesh.LengthZ();
118

119
        {
120
            // Offset fGridLen/2
121
            //
122
            _fGridLenX = (1.0f + fLengthX) / float(_ulCtGridsX);
123
            _fMinX = clBBMesh.MinX - 0.5f;
124
        }
125

126
        {
127
            _fGridLenY = (1.0f + fLengthY) / float(_ulCtGridsY);
128
            _fMinY = clBBMesh.MinY - 0.5f;
129
        }
130

131
        {
132
            _fGridLenZ = (1.0f + fLengthZ) / float(_ulCtGridsZ);
133
            _fMinZ = clBBMesh.MinZ - 0.5f;
134
        }
135
    }
136

137
    // Create data structure
138
    _aulGrid.clear();
139
    _aulGrid.resize(_ulCtGridsX);
140
    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
141
        _aulGrid[i].resize(_ulCtGridsY);
142
        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
143
            _aulGrid[i][j].resize(_ulCtGridsZ);
144
        }
145
    }
146
}
147

148
unsigned long MeshGrid::Inside(const Base::BoundBox3f& rclBB,
149
                               std::vector<ElementIndex>& raulElements,
150
                               bool bDelDoubles) const
151
{
152
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {}, ulMaxX {}, ulMaxY {}, ulMaxZ {};
153

154
    raulElements.clear();
155

156
    // Grid boxes for a more detailed selection
157
    Position(Base::Vector3f(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
158
    Position(Base::Vector3f(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
159

160
    for (auto i = ulMinX; i <= ulMaxX; i++) {
161
        for (auto j = ulMinY; j <= ulMaxY; j++) {
162
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
163
                raulElements.insert(raulElements.end(),
164
                                    _aulGrid[i][j][k].begin(),
165
                                    _aulGrid[i][j][k].end());
166
            }
167
        }
168
    }
169

170
    if (bDelDoubles) {
171
        // remove duplicate mentions
172
        std::sort(raulElements.begin(), raulElements.end());
173
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()),
174
                           raulElements.end());
175
    }
176

177
    return raulElements.size();
178
}
179

180
unsigned long MeshGrid::Inside(const Base::BoundBox3f& rclBB,
181
                               std::vector<ElementIndex>& raulElements,
182
                               const Base::Vector3f& rclOrg,
183
                               float fMaxDist,
184
                               bool bDelDoubles) const
185
{
186
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {}, ulMaxX {}, ulMaxY {}, ulMaxZ {};
187
    float fGridDiag = GetBoundBox(0, 0, 0).CalcDiagonalLength();
188
    float fMinDistP2 = (fGridDiag * fGridDiag) + (fMaxDist * fMaxDist);
189

190
    raulElements.clear();
191

192
    // Grid boxes for a more detailed selection
193
    Position(Base::Vector3f(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
194
    Position(Base::Vector3f(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
195

196
    for (auto i = ulMinX; i <= ulMaxX; i++) {
197
        for (auto j = ulMinY; j <= ulMaxY; j++) {
198
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
199
                if (Base::DistanceP2(GetBoundBox(i, j, k).GetCenter(), rclOrg) < fMinDistP2) {
200
                    raulElements.insert(raulElements.end(),
201
                                        _aulGrid[i][j][k].begin(),
202
                                        _aulGrid[i][j][k].end());
203
                }
204
            }
205
        }
206
    }
207

208
    if (bDelDoubles) {
209
        // remove duplicate mentions
210
        std::sort(raulElements.begin(), raulElements.end());
211
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()),
212
                           raulElements.end());
213
    }
214

215
    return raulElements.size();
216
}
217

218
unsigned long MeshGrid::Inside(const Base::BoundBox3f& rclBB,
219
                               std::set<ElementIndex>& raulElements) const
220
{
221
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {}, ulMaxX {}, ulMaxY {}, ulMaxZ {};
222

223
    raulElements.clear();
224

225
    // Grid boxes for a more detailed selection
226
    Position(Base::Vector3f(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
227
    Position(Base::Vector3f(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
228

229
    for (auto i = ulMinX; i <= ulMaxX; i++) {
230
        for (auto j = ulMinY; j <= ulMaxY; j++) {
231
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
232
                raulElements.insert(_aulGrid[i][j][k].begin(), _aulGrid[i][j][k].end());
233
            }
234
        }
235
    }
236

237
    return raulElements.size();
238
}
239

240
bool MeshGrid::CheckPosition(const Base::Vector3f& rclPoint,
241
                             unsigned long& rulX,
242
                             unsigned long& rulY,
243
                             unsigned long& rulZ) const
244
{
245
    rulX = static_cast<unsigned long>((rclPoint.x - _fMinX) / _fGridLenX);
246
    rulY = static_cast<unsigned long>((rclPoint.y - _fMinY) / _fGridLenY);
247
    rulZ = static_cast<unsigned long>((rclPoint.z - _fMinZ) / _fGridLenZ);
248

249
    if ((rulX < _ulCtGridsX) && (rulY < _ulCtGridsY) && (rulZ < _ulCtGridsZ)) {
250
        return true;
251
    }
252

253
    return false;
254
}
255

256
void MeshGrid::Position(const Base::Vector3f& rclPoint,
257
                        unsigned long& rulX,
258
                        unsigned long& rulY,
259
                        unsigned long& rulZ) const
260
{
261
    if (rclPoint.x <= _fMinX) {
262
        rulX = 0;
263
    }
264
    else {
265
        rulX =
266
            std::min<unsigned long>(static_cast<unsigned long>((rclPoint.x - _fMinX) / _fGridLenX),
267
                                    _ulCtGridsX - 1);
268
    }
269

270
    if (rclPoint.y <= _fMinY) {
271
        rulY = 0;
272
    }
273
    else {
274
        rulY =
275
            std::min<unsigned long>(static_cast<unsigned long>((rclPoint.y - _fMinY) / _fGridLenY),
276
                                    _ulCtGridsY - 1);
277
    }
278

279
    if (rclPoint.z <= _fMinZ) {
280
        rulZ = 0;
281
    }
282
    else {
283
        rulZ =
284
            std::min<unsigned long>(static_cast<unsigned long>((rclPoint.z - _fMinZ) / _fGridLenZ),
285
                                    _ulCtGridsZ - 1);
286
    }
287
}
288

289
void MeshGrid::CalculateGridLength(unsigned long ulCtGrid, unsigned long ulMaxGrids)
290
{
291
    // Calculate grid lengths or number of grids per dimension
292
    // There should be about 10 (?!?!) facets per grid
293
    // respectively max grids should not exceed 10000
294
    Base::BoundBox3f clBBMeshEnlarged = _pclMesh->GetBoundBox();
295
    float fGridLen = 0;
296

297
    float fLenX = clBBMeshEnlarged.LengthX();
298
    float fLenY = clBBMeshEnlarged.LengthY();
299
    float fLenZ = clBBMeshEnlarged.LengthZ();
300

301
    float fVolume = fLenX * fLenY * fLenZ;
302
    if (fVolume > 0.0f) {
303
        float fVolElem {};
304
        if (_ulCtElements > (ulMaxGrids * ulCtGrid)) {
305
            fVolElem = (fLenX * fLenY * fLenZ) / float(ulMaxGrids * ulCtGrid);
306
        }
307
        else {
308
            fVolElem = (fLenX * fLenY * fLenZ) / float(_ulCtElements);
309
        }
310

311
        float fVol = fVolElem * float(ulCtGrid);
312
        fGridLen = float(pow(fVol, 1.0f / 3.0f));
313
    }
314
    else {
315
        // Planar bounding box
316
        float fArea = fLenX * fLenY + fLenX * fLenZ + fLenY * fLenZ;
317
        float fAreaElem {};
318
        if (_ulCtElements > (ulMaxGrids * ulCtGrid)) {
319
            fAreaElem = fArea / float(ulMaxGrids * ulCtGrid);
320
        }
321
        else {
322
            fAreaElem = fArea / float(_ulCtElements);
323
        }
324

325
        float fRepresentativeArea = fAreaElem * static_cast<float>(ulCtGrid);
326
        fGridLen = sqrt(fRepresentativeArea);
327
    }
328

329
    if (fGridLen > 0) {
330
        _ulCtGridsX = std::max<unsigned long>(static_cast<unsigned long>(fLenX / fGridLen), 1);
331
        _ulCtGridsY = std::max<unsigned long>(static_cast<unsigned long>(fLenY / fGridLen), 1);
332
        _ulCtGridsZ = std::max<unsigned long>(static_cast<unsigned long>(fLenZ / fGridLen), 1);
333
    }
334
    else {
335
        // Degenerated grid
336
        _ulCtGridsX = 1;
337
        _ulCtGridsY = 1;
338
        _ulCtGridsZ = 1;
339
    }
340
}
341

342
void MeshGrid::CalculateGridLength(int iCtGridPerAxis)
343
{
344
    if (iCtGridPerAxis <= 0) {
345
        CalculateGridLength(MESH_CT_GRID, MESH_MAX_GRIDS);
346
        return;
347
    }
348

349
    // Calculate grid lengths or number of grids per dimension
350
    // There should be about 10 (?!?!) facets per grid
351
    // respectively max grids should not exceed 10000
352
    Base::BoundBox3f clBBMesh = _pclMesh->GetBoundBox();
353

354
    float fLenghtX = clBBMesh.LengthX();
355
    float fLenghtY = clBBMesh.LengthY();
356
    float fLenghtZ = clBBMesh.LengthZ();
357

358
    float fLenghtD = clBBMesh.CalcDiagonalLength();
359

360
    float fLengthTol = 0.05f * fLenghtD;
361

362
    bool bLenghtXisZero = (fLenghtX <= fLengthTol);
363
    bool bLenghtYisZero = (fLenghtY <= fLengthTol);
364
    bool bLenghtZisZero = (fLenghtZ <= fLengthTol);
365

366
    int iFlag = 0;
367

368
    int iMaxGrids = 1;
369

370
    if (bLenghtXisZero) {
371
        iFlag += 1;
372
    }
373
    else {
374
        iMaxGrids *= iCtGridPerAxis;
375
    }
376

377
    if (bLenghtYisZero) {
378
        iFlag += 2;
379
    }
380
    else {
381
        iMaxGrids *= iCtGridPerAxis;
382
    }
383

384
    if (bLenghtZisZero) {
385
        iFlag += 4;
386
    }
387
    else {
388
        iMaxGrids *= iCtGridPerAxis;
389
    }
390

391
    unsigned long ulGridsFacets = 10;
392

393
    float fFactorVolumen = 40.0;
394
    float fFactorArea = 10.0;
395

396
    switch (iFlag) {
397
        case 0: {
398
            float fVolumen = fLenghtX * fLenghtY * fLenghtZ;
399

400
            float fVolumenGrid = (fVolumen * ulGridsFacets) / (fFactorVolumen * _ulCtElements);
401

402
            if ((fVolumenGrid * iMaxGrids) < fVolumen) {
403
                fVolumenGrid = fVolumen / static_cast<float>(iMaxGrids);
404
            }
405

406
            float fLengthGrid = pow(fVolumenGrid, 1.0f / 3.0f);
407

408
            _ulCtGridsX =
409
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtX / fLengthGrid), 1);
410
            _ulCtGridsY =
411
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtY / fLengthGrid), 1);
412
            _ulCtGridsZ =
413
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtZ / fLengthGrid), 1);
414

415
        } break;
416
        case 1: {
417
            _ulCtGridsX = 1;  // bLenghtXisZero
418

419
            float fArea = fLenghtY * fLenghtZ;
420

421
            float fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
422

423
            if ((fAreaGrid * iMaxGrids) < fArea) {
424
                fAreaGrid = fArea / static_cast<float>(iMaxGrids);
425
            }
426

427
            float fLengthGrid = float(sqrt(fAreaGrid));
428

429
            _ulCtGridsY =
430
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtY / fLengthGrid), 1);
431
            _ulCtGridsZ =
432
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtZ / fLengthGrid), 1);
433
        } break;
434
        case 2: {
435
            _ulCtGridsY = 1;  // bLenghtYisZero
436

437
            float fArea = fLenghtX * fLenghtZ;
438

439
            float fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
440

441
            if ((fAreaGrid * iMaxGrids) < fArea) {
442
                fAreaGrid = fArea / static_cast<float>(iMaxGrids);
443
            }
444

445
            float fLengthGrid = float(sqrt(fAreaGrid));
446

447
            _ulCtGridsX =
448
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtX / fLengthGrid), 1);
449
            _ulCtGridsZ =
450
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtZ / fLengthGrid), 1);
451
        } break;
452
        case 3: {
453
            _ulCtGridsX = 1;                                      // bLenghtXisZero
454
            _ulCtGridsY = 1;                                      // bLenghtYisZero
455
            _ulCtGridsZ = static_cast<unsigned long>(iMaxGrids);  // bLenghtYisZero
456
        } break;
457
        case 4: {
458
            _ulCtGridsZ = 1;  // bLenghtZisZero
459

460
            float fArea = fLenghtX * fLenghtY;
461

462
            float fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
463

464
            if ((fAreaGrid * iMaxGrids) < fArea) {
465
                fAreaGrid = fArea / static_cast<float>(iMaxGrids);
466
            }
467

468
            float fLengthGrid = float(sqrt(fAreaGrid));
469

470
            _ulCtGridsX =
471
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtX / fLengthGrid), 1);
472
            _ulCtGridsY =
473
                std::max<unsigned long>(static_cast<unsigned long>(fLenghtY / fLengthGrid), 1);
474
        } break;
475
        case 5: {
476
            _ulCtGridsX = 1;                                      // bLenghtXisZero
477
            _ulCtGridsZ = 1;                                      // bLenghtZisZero
478
            _ulCtGridsY = static_cast<unsigned long>(iMaxGrids);  // bLenghtYisZero
479
        } break;
480
        case 6: {
481
            _ulCtGridsY = 1;                                      // bLenghtYisZero
482
            _ulCtGridsZ = 1;                                      // bLenghtZisZero
483
            _ulCtGridsX = static_cast<unsigned long>(iMaxGrids);  // bLenghtYisZero
484
        } break;
485
        case 7: {
486
            _ulCtGridsX = static_cast<unsigned long>(iMaxGrids);  // bLenghtXisZero
487
            _ulCtGridsY = static_cast<unsigned long>(iMaxGrids);  // bLenghtYisZero
488
            _ulCtGridsZ = static_cast<unsigned long>(iMaxGrids);  // bLenghtZisZero
489
        } break;
490
    }
491
}
492

493
void MeshGrid::SearchNearestFromPoint(const Base::Vector3f& rclPt,
494
                                      std::set<ElementIndex>& raclInd) const
495
{
496
    raclInd.clear();
497
    Base::BoundBox3f clBB = GetBoundBox();
498

499
    if (clBB.IsInBox(rclPt)) {  // Point lies within
500
        unsigned long ulX {}, ulY {}, ulZ {};
501
        Position(rclPt, ulX, ulY, ulZ);
502
        // int nX = ulX, nY = ulY, nZ = ulZ;
503
        unsigned long ulMaxLevel =
504
            std::max<unsigned long>(_ulCtGridsX, std::max<unsigned long>(_ulCtGridsY, _ulCtGridsZ));
505
        unsigned long ulLevel = 0;
506
        while (raclInd.empty() && ulLevel <= ulMaxLevel) {
507
            GetHull(ulX, ulY, ulZ, ulLevel++, raclInd);
508
        }
509
        GetHull(ulX, ulY, ulZ, ulLevel, raclInd);
510
    }
511
    else {  // Point outside
512
        Base::BoundBox3f::SIDE tSide = clBB.GetSideFromRay(rclPt, clBB.GetCenter() - rclPt);
513
        switch (tSide) {
514
            case Base::BoundBox3f::RIGHT: {
515
                unsigned long nX = 0;
516
                while (raclInd.empty() && nX < _ulCtGridsX) {
517
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
518
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
519
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
520
                        }
521
                    }
522
                    nX++;
523
                }
524
                break;
525
            }
526
            case Base::BoundBox3f::LEFT: {
527
                unsigned long nX = _ulCtGridsX - 1;
528
                while (raclInd.empty() && nX < _ulCtGridsX) {
529
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
530
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
531
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
532
                        }
533
                    }
534
                    nX++;
535
                }
536
                break;
537
            }
538
            case Base::BoundBox3f::TOP: {
539
                unsigned long nY = 0;
540
                while (raclInd.empty() && nY < _ulCtGridsY) {
541
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
542
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
543
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
544
                        }
545
                    }
546
                    nY++;
547
                }
548
                break;
549
            }
550
            case Base::BoundBox3f::BOTTOM: {
551
                unsigned long nY = _ulCtGridsY - 1;
552
                while (raclInd.empty() && nY < _ulCtGridsY) {
553
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
554
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
555
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
556
                        }
557
                    }
558
                    nY--;
559
                }
560
                break;
561
            }
562
            case Base::BoundBox3f::BACK: {
563
                unsigned long nZ = 0;
564
                while (raclInd.empty() && nZ < _ulCtGridsZ) {
565
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
566
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
567
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
568
                        }
569
                    }
570
                    nZ++;
571
                }
572
                break;
573
            }
574
            case Base::BoundBox3f::FRONT: {
575
                unsigned long nZ = _ulCtGridsZ - 1;
576
                while (raclInd.empty() && nZ < _ulCtGridsZ) {
577
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
578
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
579
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
580
                        }
581
                    }
582
                    nZ--;
583
                }
584
                break;
585
            }
586

587
            default:
588
                break;
589
        }
590
    }
591
}
592

593
void MeshGrid::GetHull(unsigned long ulX,
594
                       unsigned long ulY,
595
                       unsigned long ulZ,
596
                       unsigned long ulDistance,
597
                       std::set<ElementIndex>& raclInd) const
598
{
599
    int nX1 = std::max<int>(0, int(ulX) - int(ulDistance));
600
    int nY1 = std::max<int>(0, int(ulY) - int(ulDistance));
601
    int nZ1 = std::max<int>(0, int(ulZ) - int(ulDistance));
602
    int nX2 = std::min<int>(int(_ulCtGridsX) - 1, int(ulX) + int(ulDistance));
603
    int nY2 = std::min<int>(int(_ulCtGridsY) - 1, int(ulY) + int(ulDistance));
604
    int nZ2 = std::min<int>(int(_ulCtGridsZ) - 1, int(ulZ) + int(ulDistance));
605

606
    // top plane
607
    for (int i = nX1; i <= nX2; i++) {
608
        for (int j = nY1; j <= nY2; j++) {
609
            GetElements(static_cast<unsigned long>(i),
610
                        static_cast<unsigned long>(j),
611
                        static_cast<unsigned long>(nZ1),
612
                        raclInd);
613
        }
614
    }
615
    // bottom plane
616
    for (int i = nX1; i <= nX2; i++) {
617
        for (int j = nY1; j <= nY2; j++) {
618
            GetElements(static_cast<unsigned long>(i),
619
                        static_cast<unsigned long>(j),
620
                        static_cast<unsigned long>(nZ2),
621
                        raclInd);
622
        }
623
    }
624
    // left plane
625
    for (int i = nY1; i <= nY2; i++) {
626
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
627
            GetElements(static_cast<unsigned long>(nX1),
628
                        static_cast<unsigned long>(i),
629
                        static_cast<unsigned long>(j),
630
                        raclInd);
631
        }
632
    }
633
    // right plane
634
    for (int i = nY1; i <= nY2; i++) {
635
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
636
            GetElements(static_cast<unsigned long>(nX2),
637
                        static_cast<unsigned long>(i),
638
                        static_cast<unsigned long>(j),
639
                        raclInd);
640
        }
641
    }
642
    // front plane
643
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
644
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
645
            GetElements(static_cast<unsigned long>(i),
646
                        static_cast<unsigned long>(nY1),
647
                        static_cast<unsigned long>(j),
648
                        raclInd);
649
        }
650
    }
651
    // back plane
652
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
653
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
654
            GetElements(static_cast<unsigned long>(i),
655
                        static_cast<unsigned long>(nY2),
656
                        static_cast<unsigned long>(j),
657
                        raclInd);
658
        }
659
    }
660
}
661

662
unsigned long MeshGrid::GetElements(unsigned long ulX,
663
                                    unsigned long ulY,
664
                                    unsigned long ulZ,
665
                                    std::set<ElementIndex>& raclInd) const
666
{
667
    const std::set<ElementIndex>& rclSet = _aulGrid[ulX][ulY][ulZ];
668
    if (!rclSet.empty()) {
669
        raclInd.insert(rclSet.begin(), rclSet.end());
670
        return rclSet.size();
671
    }
672

673
    return 0;
674
}
675

676
unsigned long MeshGrid::GetElements(const Base::Vector3f& rclPoint,
677
                                    std::vector<ElementIndex>& aulFacets) const
678
{
679
    unsigned long ulX {}, ulY {}, ulZ {};
680
    if (!CheckPosition(rclPoint, ulX, ulY, ulZ)) {
681
        return 0;
682
    }
683

684
    aulFacets.resize(_aulGrid[ulX][ulY][ulZ].size());
685

686
    std::copy(_aulGrid[ulX][ulY][ulZ].begin(), _aulGrid[ulX][ulY][ulZ].end(), aulFacets.begin());
687
    return aulFacets.size();
688
}
689

690
unsigned long
691
MeshGrid::GetIndexToPosition(unsigned long ulX, unsigned long ulY, unsigned long ulZ) const
692
{
693
    if (!CheckPos(ulX, ulY, ulZ)) {
694
        return ULONG_MAX;
695
    }
696
    return (ulZ * _ulCtGridsY + ulY) * _ulCtGridsX + ulX;
697
}
698

699
bool MeshGrid::GetPositionToIndex(unsigned long id,
700
                                  unsigned long& ulX,
701
                                  unsigned long& ulY,
702
                                  unsigned long& ulZ) const
703
{
704
    ulX = id % _ulCtGridsX;
705
    ulY = (id / _ulCtGridsX) % _ulCtGridsY;
706
    ulZ = id / (_ulCtGridsX * _ulCtGridsY);
707

708
    if (!CheckPos(ulX, ulY, ulZ)) {
709
        ulX = ULONG_MAX;
710
        ulY = ULONG_MAX;
711
        ulZ = ULONG_MAX;
712
        return false;
713
    }
714

715
    return true;
716
}
717

718
// ----------------------------------------------------------------
719

720
MeshFacetGrid::MeshFacetGrid(const MeshKernel& rclM)
721
    : MeshGrid(rclM)
722
{
723
    MeshFacetGrid::RebuildGrid();
724
}
725

726
MeshFacetGrid::MeshFacetGrid(const MeshKernel& rclM, int iCtGridPerAxis)
727
    : MeshGrid(rclM)
728
{
729
    Rebuild(iCtGridPerAxis);
730
}
731

732
MeshFacetGrid::MeshFacetGrid(const MeshKernel& rclM,
733
                             unsigned long ulX,
734
                             unsigned long ulY,
735
                             unsigned long ulZ)
736
    : MeshGrid(rclM)
737
{
738
    Rebuild(ulX, ulY, ulZ);
739
}
740

741
MeshFacetGrid::MeshFacetGrid(const MeshKernel& rclM, float fGridLen)
742
    : MeshGrid(rclM)
743
{
744
    Base::BoundBox3f clBBMesh = _pclMesh->GetBoundBox();
745
    Rebuild(std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthX() / fGridLen), 1),
746
            std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthY() / fGridLen), 1),
747
            std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthZ() / fGridLen), 1));
748
}
749

750
void MeshFacetGrid::Validate(const MeshKernel& rclMesh)
751
{
752
    if (_pclMesh != &rclMesh) {
753
        Attach(rclMesh);
754
    }
755
    else if (rclMesh.CountFacets() != _ulCtElements) {
756
        RebuildGrid();
757
    }
758
}
759

760
void MeshFacetGrid::Validate()
761
{
762
    if (!_pclMesh) {
763
        return;
764
    }
765

766
    if (_pclMesh->CountFacets() != _ulCtElements) {
767
        RebuildGrid();
768
    }
769
}
770

771
bool MeshFacetGrid::Verify() const
772
{
773
    if (!_pclMesh) {
774
        return false;  // no mesh attached
775
    }
776
    if (_pclMesh->CountFacets() != _ulCtElements) {
777
        return false;  // not up-to-date
778
    }
779

780
    MeshGridIterator it(*this);
781
    MeshFacetIterator cF(*_pclMesh);
782
    for (it.Init(); it.More(); it.Next()) {
783
        std::vector<ElementIndex> aulElements;
784
        it.GetElements(aulElements);
785
        for (ElementIndex element : aulElements) {
786
            cF.Set(element);
787
            if (!cF->IntersectBoundingBox(it.GetBoundBox())) {
788
                return false;  // no intersection between facet although the facet is in grid
789
            }
790
        }
791
    }
792

793
    return true;
794
}
795

796
void MeshFacetGrid::RebuildGrid()
797
{
798
    _ulCtElements = _pclMesh->CountFacets();
799

800
    InitGrid();
801

802
    // Fill data structure
803
    MeshFacetIterator clFIter(*_pclMesh);
804

805
    unsigned long i = 0;
806
    for (clFIter.Init(); clFIter.More(); clFIter.Next()) {
807
        //    AddFacet(*clFIter, i++, 2.0f);
808
        AddFacet(*clFIter, i++);
809
    }
810
}
811

812
unsigned long MeshFacetGrid::SearchNearestFromPoint(const Base::Vector3f& rclPt) const
813
{
814
    ElementIndex ulFacetInd = ELEMENT_INDEX_MAX;
815
    float fMinDist = FLOAT_MAX;
816
    Base::BoundBox3f clBB = GetBoundBox();
817

818
    if (clBB.IsInBox(rclPt)) {  // Point lies within
819
        unsigned long ulX {}, ulY {}, ulZ {};
820
        Position(rclPt, ulX, ulY, ulZ);
821
        float fMinGridDist = std::min<float>(std::min<float>(_fGridLenX, _fGridLenY), _fGridLenZ);
822
        unsigned long ulDistance = 0;
823
        while (fMinDist > (fMinGridDist * float(ulDistance))) {
824
            SearchNearestFacetInHull(ulX, ulY, ulZ, ulDistance, rclPt, ulFacetInd, fMinDist);
825
            ulDistance++;
826
        }
827
        SearchNearestFacetInHull(ulX, ulY, ulZ, ulDistance, rclPt, ulFacetInd, fMinDist);
828
    }
829
    else {  // Point outside
830
        Base::BoundBox3f::SIDE tSide = clBB.GetSideFromRay(rclPt, clBB.GetCenter() - rclPt);
831
        switch (tSide) {
832
            case Base::BoundBox3f::RIGHT: {
833
                unsigned long nX = 0;
834
                while ((fMinDist > ((clBB.MinX - rclPt.x) + (float(nX) * _fGridLenX)))
835
                       && (nX < _ulCtGridsX)) {
836
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
837
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
838
                            SearchNearestFacetInGrid(nX, i, j, rclPt, fMinDist, ulFacetInd);
839
                        }
840
                    }
841
                    nX++;
842
                }
843
                break;
844
            }
845
            case Base::BoundBox3f::LEFT: {
846
                unsigned long nX = _ulCtGridsX - 1;
847
                while ((fMinDist > ((rclPt.x - clBB.MinX) + (float(nX) * _fGridLenX)))
848
                       && (nX < _ulCtGridsX)) {
849
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
850
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
851
                            SearchNearestFacetInGrid(nX, i, j, rclPt, fMinDist, ulFacetInd);
852
                        }
853
                    }
854
                    nX--;
855
                }
856
                break;
857
            }
858
            case Base::BoundBox3f::TOP: {
859
                unsigned long nY = 0;
860
                while ((fMinDist > ((clBB.MinY - rclPt.y) + (float(nY) * _fGridLenY)))
861
                       && (nY < _ulCtGridsY)) {
862
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
863
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
864
                            SearchNearestFacetInGrid(i, nY, j, rclPt, fMinDist, ulFacetInd);
865
                        }
866
                    }
867
                    nY++;
868
                }
869
                break;
870
            }
871
            case Base::BoundBox3f::BOTTOM: {
872
                unsigned long nY = _ulCtGridsY - 1;
873
                while ((fMinDist > ((rclPt.y - clBB.MinY) + (float(nY) * _fGridLenY)))
874
                       && (nY < _ulCtGridsY)) {
875
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
876
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
877
                            SearchNearestFacetInGrid(i, nY, j, rclPt, fMinDist, ulFacetInd);
878
                        }
879
                    }
880
                    nY--;
881
                }
882
                break;
883
            }
884
            case Base::BoundBox3f::BACK: {
885
                unsigned long nZ = 0;
886
                while ((fMinDist > ((clBB.MinZ - rclPt.z) + (float(nZ) * _fGridLenZ)))
887
                       && (nZ < _ulCtGridsZ)) {
888
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
889
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
890
                            SearchNearestFacetInGrid(i, j, nZ, rclPt, fMinDist, ulFacetInd);
891
                        }
892
                    }
893
                    nZ++;
894
                }
895
                break;
896
            }
897
            case Base::BoundBox3f::FRONT: {
898
                unsigned long nZ = _ulCtGridsZ - 1;
899
                while ((fMinDist > ((rclPt.z - clBB.MinZ) + (float(nZ) * _fGridLenZ)))
900
                       && (nZ < _ulCtGridsZ)) {
901
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
902
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
903
                            SearchNearestFacetInGrid(i, j, nZ, rclPt, fMinDist, ulFacetInd);
904
                        }
905
                    }
906
                    nZ--;
907
                }
908
                break;
909
            }
910
            default:
911
                break;
912
        }
913
    }
914

915
    return ulFacetInd;
916
}
917

918
unsigned long MeshFacetGrid::SearchNearestFromPoint(const Base::Vector3f& rclPt,
919
                                                    float fMaxSearchArea) const
920
{
921
    std::vector<ElementIndex> aulFacets;
922
    ElementIndex ulFacetInd = ELEMENT_INDEX_MAX;
923
    float fMinDist = fMaxSearchArea;
924

925
    MeshAlgorithm clFTool(*_pclMesh);
926

927
    Base::BoundBox3f clBB(rclPt.x - fMaxSearchArea,
928
                          rclPt.y - fMaxSearchArea,
929
                          rclPt.z - fMaxSearchArea,
930
                          rclPt.x + fMaxSearchArea,
931
                          rclPt.y + fMaxSearchArea,
932
                          rclPt.z + fMaxSearchArea);
933

934
    Inside(clBB, aulFacets, rclPt, fMaxSearchArea, true);
935

936
    for (ElementIndex facet : aulFacets) {
937
        float fDist {};
938

939
        if (clFTool.Distance(rclPt, facet, fMinDist, fDist)) {
940
            fMinDist = fDist;
941
            ulFacetInd = facet;
942
        }
943
    }
944

945
    return ulFacetInd;
946
}
947

948
void MeshFacetGrid::SearchNearestFacetInHull(unsigned long ulX,
949
                                             unsigned long ulY,
950
                                             unsigned long ulZ,
951
                                             unsigned long ulDistance,
952
                                             const Base::Vector3f& rclPt,
953
                                             ElementIndex& rulFacetInd,
954
                                             float& rfMinDist) const
955
{
956
    int nX1 = std::max<int>(0, int(ulX) - int(ulDistance));
957
    int nY1 = std::max<int>(0, int(ulY) - int(ulDistance));
958
    int nZ1 = std::max<int>(0, int(ulZ) - int(ulDistance));
959
    int nX2 = std::min<int>(int(_ulCtGridsX) - 1, int(ulX) + int(ulDistance));
960
    int nY2 = std::min<int>(int(_ulCtGridsY) - 1, int(ulY) + int(ulDistance));
961
    int nZ2 = std::min<int>(int(_ulCtGridsZ) - 1, int(ulZ) + int(ulDistance));
962

963
    // top plane
964
    for (int i = nX1; i <= nX2; i++) {
965
        for (int j = nY1; j <= nY2; j++) {
966
            SearchNearestFacetInGrid(static_cast<unsigned long>(i),
967
                                     static_cast<unsigned long>(j),
968
                                     static_cast<unsigned long>(nZ1),
969
                                     rclPt,
970
                                     rfMinDist,
971
                                     rulFacetInd);
972
        }
973
    }
974
    // bottom plane
975
    for (int i = nX1; i <= nX2; i++) {
976
        for (int j = nY1; j <= nY2; j++) {
977
            SearchNearestFacetInGrid(static_cast<unsigned long>(i),
978
                                     static_cast<unsigned long>(j),
979
                                     static_cast<unsigned long>(nZ2),
980
                                     rclPt,
981
                                     rfMinDist,
982
                                     rulFacetInd);
983
        }
984
    }
985
    // left plane
986
    for (int i = nY1; i <= nY2; i++) {
987
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
988
            SearchNearestFacetInGrid(static_cast<unsigned long>(nX1),
989
                                     static_cast<unsigned long>(i),
990
                                     static_cast<unsigned long>(j),
991
                                     rclPt,
992
                                     rfMinDist,
993
                                     rulFacetInd);
994
        }
995
    }
996
    // right plane
997
    for (int i = nY1; i <= nY2; i++) {
998
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
999
            SearchNearestFacetInGrid(static_cast<unsigned long>(nX2),
1000
                                     static_cast<unsigned long>(i),
1001
                                     static_cast<unsigned long>(j),
1002
                                     rclPt,
1003
                                     rfMinDist,
1004
                                     rulFacetInd);
1005
        }
1006
    }
1007
    // front plane
1008
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
1009
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
1010
            SearchNearestFacetInGrid(static_cast<unsigned long>(i),
1011
                                     static_cast<unsigned long>(nY1),
1012
                                     static_cast<unsigned long>(j),
1013
                                     rclPt,
1014
                                     rfMinDist,
1015
                                     rulFacetInd);
1016
        }
1017
    }
1018
    // back plane
1019
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
1020
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
1021
            SearchNearestFacetInGrid(static_cast<unsigned long>(i),
1022
                                     static_cast<unsigned long>(nY2),
1023
                                     static_cast<unsigned long>(j),
1024
                                     rclPt,
1025
                                     rfMinDist,
1026
                                     rulFacetInd);
1027
        }
1028
    }
1029
}
1030

1031
void MeshFacetGrid::SearchNearestFacetInGrid(unsigned long ulX,
1032
                                             unsigned long ulY,
1033
                                             unsigned long ulZ,
1034
                                             const Base::Vector3f& rclPt,
1035
                                             float& rfMinDist,
1036
                                             ElementIndex& rulFacetInd) const
1037
{
1038
    const std::set<ElementIndex>& rclSet = _aulGrid[ulX][ulY][ulZ];
1039
    for (ElementIndex pI : rclSet) {
1040
        float fDist = _pclMesh->GetFacet(pI).DistanceToPoint(rclPt);
1041
        if (fDist < rfMinDist) {
1042
            rfMinDist = fDist;
1043
            rulFacetInd = pI;
1044
        }
1045
    }
1046
}
1047

1048
//----------------------------------------------------------------------------
1049

1050
MeshPointGrid::MeshPointGrid(const MeshKernel& rclM)
1051
    : MeshGrid(rclM)
1052
{
1053
    MeshPointGrid::RebuildGrid();
1054
}
1055

1056
MeshPointGrid::MeshPointGrid()
1057
    : MeshGrid()
1058
{}
1059

1060
MeshPointGrid::MeshPointGrid(const MeshKernel& rclM,
1061
                             unsigned long ulX,
1062
                             unsigned long ulY,
1063
                             unsigned long ulZ)
1064
    : MeshGrid(rclM)
1065
{
1066
    Rebuild(ulX, ulY, ulZ);
1067
}
1068

1069
MeshPointGrid::MeshPointGrid(const MeshKernel& rclM, int iCtGridPerAxis)
1070
    : MeshGrid(rclM)
1071
{
1072
    Rebuild(iCtGridPerAxis);
1073
}
1074

1075
MeshPointGrid::MeshPointGrid(const MeshKernel& rclM, float fGridLen)
1076
    : MeshGrid(rclM)
1077
{
1078
    Base::BoundBox3f clBBMesh = _pclMesh->GetBoundBox();
1079
    Rebuild(std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthX() / fGridLen), 1),
1080
            std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthY() / fGridLen), 1),
1081
            std::max<unsigned long>(static_cast<unsigned long>(clBBMesh.LengthZ() / fGridLen), 1));
1082
}
1083

1084
void MeshPointGrid::AddPoint(const MeshPoint& rclPt, ElementIndex ulPtIndex, float fEpsilon)
1085
{
1086
    (void)fEpsilon;
1087
    unsigned long ulX {}, ulY {}, ulZ {};
1088
    Pos(Base::Vector3f(rclPt.x, rclPt.y, rclPt.z), ulX, ulY, ulZ);
1089
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
1090
        _aulGrid[ulX][ulY][ulZ].insert(ulPtIndex);
1091
    }
1092
}
1093

1094
void MeshPointGrid::Validate(const MeshKernel& rclMesh)
1095
{
1096
    if (_pclMesh != &rclMesh) {
1097
        Attach(rclMesh);
1098
    }
1099
    else if (rclMesh.CountPoints() != _ulCtElements) {
1100
        RebuildGrid();
1101
    }
1102
}
1103

1104
void MeshPointGrid::Validate()
1105
{
1106
    if (!_pclMesh) {
1107
        return;
1108
    }
1109

1110
    if (_pclMesh->CountPoints() != _ulCtElements) {
1111
        RebuildGrid();
1112
    }
1113
}
1114

1115
bool MeshPointGrid::Verify() const
1116
{
1117
    if (!_pclMesh) {
1118
        return false;  // no mesh attached
1119
    }
1120
    if (_pclMesh->CountFacets() != _ulCtElements) {
1121
        return false;  // not up-to-date
1122
    }
1123

1124
    MeshGridIterator it(*this);
1125
    MeshPointIterator cP(*_pclMesh);
1126
    for (it.Init(); it.More(); it.Next()) {
1127
        std::vector<ElementIndex> aulElements;
1128
        it.GetElements(aulElements);
1129
        for (ElementIndex element : aulElements) {
1130
            cP.Set(element);
1131
            if (!it.GetBoundBox().IsInBox(*cP)) {
1132
                return false;  // point doesn't lie inside the grid element
1133
            }
1134
        }
1135
    }
1136

1137
    return true;
1138
}
1139

1140
void MeshPointGrid::RebuildGrid()
1141
{
1142
    _ulCtElements = _pclMesh->CountPoints();
1143

1144
    InitGrid();
1145

1146
    // Fill data structure
1147

1148
    MeshPointIterator cPIter(*_pclMesh);
1149

1150
    unsigned long i = 0;
1151
    for (cPIter.Init(); cPIter.More(); cPIter.Next()) {
1152
        AddPoint(*cPIter, i++);
1153
    }
1154
}
1155

1156
void MeshPointGrid::Pos(const Base::Vector3f& rclPoint,
1157
                        unsigned long& rulX,
1158
                        unsigned long& rulY,
1159
                        unsigned long& rulZ) const
1160
{
1161
    rulX = static_cast<unsigned long>((rclPoint.x - _fMinX) / _fGridLenX);
1162
    rulY = static_cast<unsigned long>((rclPoint.y - _fMinY) / _fGridLenY);
1163
    rulZ = static_cast<unsigned long>((rclPoint.z - _fMinZ) / _fGridLenZ);
1164
}
1165

1166
unsigned long MeshPointGrid::FindElements(const Base::Vector3f& rclPoint,
1167
                                          std::set<ElementIndex>& aulElements) const
1168
{
1169
    unsigned long ulX {}, ulY {}, ulZ {};
1170
    Pos(rclPoint, ulX, ulY, ulZ);
1171

1172
    // check if the given point is inside the grid structure
1173
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
1174
        return GetElements(ulX, ulY, ulZ, aulElements);
1175
    }
1176

1177
    return 0;
1178
}
1179

1180
// ----------------------------------------------------------------
1181

1182
MeshGridIterator::MeshGridIterator(const MeshGrid& rclG)
1183
    : _rclGrid(rclG)
1184
    , _clPt(0.0f, 0.0f, 0.0f)
1185
    , _clDir(0.0f, 0.0f, 0.0f)
1186
{}
1187

1188
bool MeshGridIterator::InitOnRay(const Base::Vector3f& rclPt,
1189
                                 const Base::Vector3f& rclDir,
1190
                                 float fMaxSearchArea,
1191
                                 std::vector<ElementIndex>& raulElements)
1192
{
1193
    bool ret = InitOnRay(rclPt, rclDir, raulElements);
1194
    _fMaxSearchArea = fMaxSearchArea;
1195
    return ret;
1196
}
1197

1198
bool MeshGridIterator::InitOnRay(const Base::Vector3f& rclPt,
1199
                                 const Base::Vector3f& rclDir,
1200
                                 std::vector<ElementIndex>& raulElements)
1201
{
1202
    // needed in NextOnRay() to avoid an infinite loop
1203
    _cSearchPositions.clear();
1204

1205
    _fMaxSearchArea = FLOAT_MAX;
1206

1207
    raulElements.clear();
1208

1209
    _clPt = rclPt;
1210
    _clDir = rclDir;
1211
    _bValidRay = false;
1212

1213
    // point lies within global BB
1214
    if (_rclGrid.GetBoundBox().IsInBox(rclPt)) {  // Determine the voxel by the starting point
1215
        _rclGrid.Position(rclPt, _ulX, _ulY, _ulZ);
1216
        raulElements.insert(raulElements.end(),
1217
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
1218
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
1219
        _bValidRay = true;
1220
    }
1221
    else {  // Start point outside
1222
        Base::Vector3f cP0, cP1;
1223
        if (_rclGrid.GetBoundBox().IntersectWithLine(rclPt,
1224
                                                     rclDir,
1225
                                                     cP0,
1226
                                                     cP1)) {  // determine the next point
1227
            if ((cP0 - rclPt).Length() < (cP1 - rclPt).Length()) {
1228
                _rclGrid.Position(cP0, _ulX, _ulY, _ulZ);
1229
            }
1230
            else {
1231
                _rclGrid.Position(cP1, _ulX, _ulY, _ulZ);
1232
            }
1233

1234
            raulElements.insert(raulElements.end(),
1235
                                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
1236
                                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
1237
            _bValidRay = true;
1238
        }
1239
    }
1240

1241
    return _bValidRay;
1242
}
1243

1244
bool MeshGridIterator::NextOnRay(std::vector<ElementIndex>& raulElements)
1245
{
1246
    if (!_bValidRay) {
1247
        return false;  // not initialized or beam exited
1248
    }
1249

1250
    raulElements.clear();
1251

1252
    Base::Vector3f clIntersectPoint;
1253

1254
    // Look for the next adjacent BB on the search beam
1255
    Base::BoundBox3f::SIDE tSide =
1256
        _rclGrid.GetBoundBox(_ulX, _ulY, _ulZ).GetSideFromRay(_clPt, _clDir, clIntersectPoint);
1257

1258
    // Search area
1259
    //
1260
    if ((_clPt - clIntersectPoint).Length() > _fMaxSearchArea) {
1261
        _bValidRay = false;
1262
    }
1263
    else {
1264
        switch (tSide) {
1265
            case Base::BoundBox3f::LEFT:
1266
                _ulX--;
1267
                break;
1268
            case Base::BoundBox3f::RIGHT:
1269
                _ulX++;
1270
                break;
1271
            case Base::BoundBox3f::BOTTOM:
1272
                _ulY--;
1273
                break;
1274
            case Base::BoundBox3f::TOP:
1275
                _ulY++;
1276
                break;
1277
            case Base::BoundBox3f::FRONT:
1278
                _ulZ--;
1279
                break;
1280
            case Base::BoundBox3f::BACK:
1281
                _ulZ++;
1282
                break;
1283

1284
            default:
1285
            case Base::BoundBox3f::INVALID:
1286
                _bValidRay = false;
1287
                break;
1288
        }
1289

1290
        GridElement pos(_ulX, _ulY, _ulZ);
1291
        if (_cSearchPositions.find(pos) != _cSearchPositions.end()) {
1292
            _bValidRay =
1293
                false;  // grid element already visited => result from GetSideFromRay invalid
1294
        }
1295
    }
1296

1297
    if (_bValidRay && _rclGrid.CheckPos(_ulX, _ulY, _ulZ)) {
1298
        GridElement pos(_ulX, _ulY, _ulZ);
1299
        _cSearchPositions.insert(pos);
1300
        raulElements.insert(raulElements.end(),
1301
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
1302
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
1303
    }
1304
    else {
1305
        _bValidRay = false;  // Beam leaked
1306
    }
1307

1308
    return _bValidRay;
1309
}
1310