FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2004 Werner Mayer <wmayer[at]users.sourceforge.net>     *
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 "PointsGrid.h"
26

27

28
using namespace Points;
29

30
PointsGrid::PointsGrid(const PointKernel& rclM)
31
    : _pclPoints(&rclM)
32
    , _ulCtElements(0)
33
    , _ulCtGridsX(0)
34
    , _ulCtGridsY(0)
35
    , _ulCtGridsZ(0)
36
    , _fGridLenX(0.0F)
37
    , _fGridLenY(0.0F)
38
    , _fGridLenZ(0.0F)
39
    , _fMinX(0.0F)
40
    , _fMinY(0.0F)
41
    , _fMinZ(0.0F)
42
{
43
    PointsGrid::RebuildGrid();
44
}
45

46
PointsGrid::PointsGrid()
47
    : _pclPoints(nullptr)
48
    , _ulCtElements(0)
49
    , _ulCtGridsX(POINTS_CT_GRID)
50
    , _ulCtGridsY(POINTS_CT_GRID)
51
    , _ulCtGridsZ(POINTS_CT_GRID)
52
    , _fGridLenX(0.0F)
53
    , _fGridLenY(0.0F)
54
    , _fGridLenZ(0.0F)
55
    , _fMinX(0.0F)
56
    , _fMinY(0.0F)
57
    , _fMinZ(0.0F)
58
{}
59

60
PointsGrid::PointsGrid(const PointKernel& rclM,
61
                       unsigned long ulX,
62
                       unsigned long ulY,
63
                       unsigned long ulZ)
64
    : _pclPoints(&rclM)
65
    , _ulCtElements(0)
66
    , _ulCtGridsX(0)
67
    , _ulCtGridsY(0)
68
    , _ulCtGridsZ(0)
69
    , _fGridLenX(0.0F)
70
    , _fGridLenY(0.0F)
71
    , _fGridLenZ(0.0F)
72
    , _fMinX(0.0F)
73
    , _fMinY(0.0F)
74
    , _fMinZ(0.0F)
75
{
76
    PointsGrid::Rebuild(ulX, ulY, ulZ);
77
}
78

79
PointsGrid::PointsGrid(const PointKernel& rclM, int iCtGridPerAxis)
80
    : _pclPoints(&rclM)
81
    , _ulCtElements(0)
82
    , _ulCtGridsX(0)
83
    , _ulCtGridsY(0)
84
    , _ulCtGridsZ(0)
85
    , _fGridLenX(0.0F)
86
    , _fGridLenY(0.0F)
87
    , _fGridLenZ(0.0F)
88
    , _fMinX(0.0F)
89
    , _fMinY(0.0F)
90
    , _fMinZ(0.0F)
91
{
92
    PointsGrid::Rebuild(iCtGridPerAxis);
93
}
94

95
PointsGrid::PointsGrid(const PointKernel& rclM, double fGridLen)
96
    : _pclPoints(&rclM)
97
    , _ulCtElements(0)
98
    , _ulCtGridsX(0)
99
    , _ulCtGridsY(0)
100
    , _ulCtGridsZ(0)
101
    , _fGridLenX(0.0F)
102
    , _fGridLenY(0.0F)
103
    , _fGridLenZ(0.0F)
104
    , _fMinX(0.0F)
105
    , _fMinY(0.0F)
106
    , _fMinZ(0.0F)
107
{
108
    Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
109
    for (const auto& pnt : *_pclPoints) {
110
        clBBPts.Add(pnt);
111
    }
112
    PointsGrid::Rebuild(std::max<unsigned long>((unsigned long)(clBBPts.LengthX() / fGridLen), 1),
113
                        std::max<unsigned long>((unsigned long)(clBBPts.LengthY() / fGridLen), 1),
114
                        std::max<unsigned long>((unsigned long)(clBBPts.LengthZ() / fGridLen), 1));
115
}
116

117
void PointsGrid::Attach(const PointKernel& rclM)
118
{
119
    _pclPoints = &rclM;
120
    RebuildGrid();
121
}
122

123
void PointsGrid::Clear()
124
{
125
    _aulGrid.clear();
126
    _pclPoints = nullptr;
127
}
128

129
void PointsGrid::Rebuild(unsigned long ulX, unsigned long ulY, unsigned long ulZ)
130
{
131
    _ulCtGridsX = ulX;
132
    _ulCtGridsY = ulY;
133
    _ulCtGridsZ = ulZ;
134
    _ulCtElements = HasElements();
135
    RebuildGrid();
136
}
137

138
void PointsGrid::Rebuild(unsigned long ulPerGrid, unsigned long ulMaxGrid)
139
{
140
    _ulCtElements = HasElements();
141
    CalculateGridLength(ulPerGrid, ulMaxGrid);
142
    RebuildGrid();
143
}
144

145
void PointsGrid::Rebuild(int iCtGridPerAxis)
146
{
147
    _ulCtElements = HasElements();
148
    CalculateGridLength(iCtGridPerAxis);
149
    RebuildGrid();
150
}
151

152
void PointsGrid::InitGrid()
153
{
154
    assert(_pclPoints);
155

156
    // Calculate grid lengths if not initialized
157
    //
158
    if ((_ulCtGridsX == 0) || (_ulCtGridsY == 0) || (_ulCtGridsZ == 0)) {
159
        CalculateGridLength(POINTS_CT_GRID, POINTS_MAX_GRIDS);
160
    }
161

162
    // Determine the grid length and offset
163
    //
164
    {
165
        Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
166
        for (const auto& pnt : *_pclPoints) {
167
            clBBPts.Add(pnt);
168
        }
169

170
        double fLengthX = clBBPts.LengthX();
171
        double fLengthY = clBBPts.LengthY();
172
        double fLengthZ = clBBPts.LengthZ();
173

174
        {
175
            // Offset fGridLen/2
176
            //
177
            unsigned long num = _ulCtGridsX;
178
            if (num == 0) {
179
                num = 1;
180
            }
181
            _fGridLenX = (1.0F + fLengthX) / double(num);
182
            _fMinX = clBBPts.MinX - 0.5F;
183
        }
184

185
        {
186
            unsigned long num = _ulCtGridsY;
187
            if (num == 0) {
188
                num = 1;
189
            }
190
            _fGridLenY = (1.0F + fLengthY) / double(num);
191
            _fMinY = clBBPts.MinY - 0.5F;
192
        }
193

194
        {
195
            unsigned long num = _ulCtGridsZ;
196
            if (num == 0) {
197
                num = 1;
198
            }
199
            _fGridLenZ = (1.0F + fLengthZ) / double(num);
200
            _fMinZ = clBBPts.MinZ - 0.5F;
201
        }
202
    }
203

204
    // Create data structure
205
    _aulGrid.clear();
206
    _aulGrid.resize(_ulCtGridsX);
207
    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
208
        _aulGrid[i].resize(_ulCtGridsY);
209
        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
210
            _aulGrid[i][j].resize(_ulCtGridsZ);
211
        }
212
    }
213
}
214

215
unsigned long PointsGrid::InSide(const Base::BoundBox3d& rclBB,
216
                                 std::vector<unsigned long>& raulElements,
217
                                 bool bDelDoubles) const
218
{
219
    unsigned long ulMinX {};
220
    unsigned long ulMinY {};
221
    unsigned long ulMinZ {};
222
    unsigned long ulMaxX {};
223
    unsigned long ulMaxY {};
224
    unsigned long ulMaxZ {};
225

226
    raulElements.clear();
227

228
    // Grid boxes for a more detailed selection
229
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
230
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
231

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

242
    if (bDelDoubles) {
243
        // remove duplicate mentions
244
        std::sort(raulElements.begin(), raulElements.end());
245
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()),
246
                           raulElements.end());
247
    }
248

249
    return raulElements.size();
250
}
251

252
unsigned long PointsGrid::InSide(const Base::BoundBox3d& rclBB,
253
                                 std::vector<unsigned long>& raulElements,
254
                                 const Base::Vector3d& rclOrg,
255
                                 double fMaxDist,
256
                                 bool bDelDoubles) const
257
{
258
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {};
259
    unsigned long ulMaxX {}, ulMaxY {}, ulMaxZ {};
260
    double fGridDiag = GetBoundBox(0, 0, 0).CalcDiagonalLength();
261
    double fMinDistP2 = (fGridDiag * fGridDiag) + (fMaxDist * fMaxDist);
262

263
    raulElements.clear();
264

265
    // Grid boxes for a more detailed selection
266
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
267
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
268

269
    for (auto i = ulMinX; i <= ulMaxX; i++) {
270
        for (auto j = ulMinY; j <= ulMaxY; j++) {
271
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
272
                if (Base::DistanceP2(GetBoundBox(i, j, k).GetCenter(), rclOrg) < fMinDistP2) {
273
                    raulElements.insert(raulElements.end(),
274
                                        _aulGrid[i][j][k].begin(),
275
                                        _aulGrid[i][j][k].end());
276
                }
277
            }
278
        }
279
    }
280

281
    if (bDelDoubles) {
282
        // remove duplicate mentions
283
        std::sort(raulElements.begin(), raulElements.end());
284
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()),
285
                           raulElements.end());
286
    }
287

288
    return raulElements.size();
289
}
290

291
unsigned long PointsGrid::InSide(const Base::BoundBox3d& rclBB,
292
                                 std::set<unsigned long>& raulElements) const
293
{
294
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {};
295
    unsigned long ulMaxX {}, ulMaxY {}, ulMaxZ {};
296

297
    raulElements.clear();
298

299
    // Grid boxes for a more detailed selection
300
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
301
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);
302

303
    for (auto i = ulMinX; i <= ulMaxX; i++) {
304
        for (auto j = ulMinY; j <= ulMaxY; j++) {
305
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
306
                raulElements.insert(_aulGrid[i][j][k].begin(), _aulGrid[i][j][k].end());
307
            }
308
        }
309
    }
310

311
    return raulElements.size();
312
}
313

314
void PointsGrid::Position(const Base::Vector3d& rclPoint,
315
                          unsigned long& rulX,
316
                          unsigned long& rulY,
317
                          unsigned long& rulZ) const
318
{
319
    if (rclPoint.x <= _fMinX) {
320
        rulX = 0;
321
    }
322
    else {
323
        rulX = std::min<unsigned long>((unsigned long)((rclPoint.x - _fMinX) / _fGridLenX),
324
                                       _ulCtGridsX - 1);
325
    }
326

327
    if (rclPoint.y <= _fMinY) {
328
        rulY = 0;
329
    }
330
    else {
331
        rulY = std::min<unsigned long>((unsigned long)((rclPoint.y - _fMinY) / _fGridLenY),
332
                                       _ulCtGridsY - 1);
333
    }
334

335
    if (rclPoint.z <= _fMinZ) {
336
        rulZ = 0;
337
    }
338
    else {
339
        rulZ = std::min<unsigned long>((unsigned long)((rclPoint.z - _fMinZ) / _fGridLenZ),
340
                                       _ulCtGridsZ - 1);
341
    }
342
}
343

344
void PointsGrid::CalculateGridLength(unsigned long ulCtGrid, unsigned long ulMaxGrids)
345
{
346
    // Calculate grid lengths or number of grids per dimension
347
    // There should be about 10 (?!?!) facets per grid
348
    // or max grids should not exceed 10000
349
    Base::BoundBox3d clBBPtsEnlarged;  // = _pclPoints->GetBoundBox();
350
    for (const auto& pnt : *_pclPoints) {
351
        clBBPtsEnlarged.Add(pnt);
352
    }
353
    double fVolElem {};
354

355
    if (_ulCtElements > (ulMaxGrids * ulCtGrid)) {
356
        fVolElem =
357
            (clBBPtsEnlarged.LengthX() * clBBPtsEnlarged.LengthY() * clBBPtsEnlarged.LengthZ())
358
            / float(ulMaxGrids * ulCtGrid);
359
    }
360
    else {
361
        fVolElem =
362
            (clBBPtsEnlarged.LengthX() * clBBPtsEnlarged.LengthY() * clBBPtsEnlarged.LengthZ())
363
            / float(_ulCtElements);
364
    }
365

366
    double fVol = fVolElem * float(ulCtGrid);
367
    double fGridLen = float(pow((float)fVol, 1.0F / 3.0F));
368

369
    if (fGridLen > 0) {
370
        _ulCtGridsX =
371
            std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthX() / fGridLen), 1);
372
        _ulCtGridsY =
373
            std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthY() / fGridLen), 1);
374
        _ulCtGridsZ =
375
            std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthZ() / fGridLen), 1);
376
    }
377
    else {
378
        // Degenerated grid
379
        _ulCtGridsX = 1;
380
        _ulCtGridsY = 1;
381
        _ulCtGridsZ = 1;
382
    }
383
}
384

385
void PointsGrid::CalculateGridLength(int iCtGridPerAxis)
386
{
387
    if (iCtGridPerAxis <= 0) {
388
        CalculateGridLength(POINTS_CT_GRID, POINTS_MAX_GRIDS);
389
        return;
390
    }
391

392
    // Calculate grid lengths or number of grids per dimension
393
    // There should be about 10 (?!?!) facets per grid
394
    // or max grids should not exceed 10000
395
    Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
396
    for (const auto& pnt : *_pclPoints) {
397
        clBBPts.Add(pnt);
398
    }
399

400
    double fLenghtX = clBBPts.LengthX();
401
    double fLenghtY = clBBPts.LengthY();
402
    double fLenghtZ = clBBPts.LengthZ();
403

404
    double fLenghtD = clBBPts.CalcDiagonalLength();
405

406
    double fLengthTol = 0.05F * fLenghtD;
407

408
    bool bLenghtXisZero = (fLenghtX <= fLengthTol);
409
    bool bLenghtYisZero = (fLenghtY <= fLengthTol);
410
    bool bLenghtZisZero = (fLenghtZ <= fLengthTol);
411

412
    int iFlag = 0;
413

414
    int iMaxGrids = 1;
415

416
    if (bLenghtXisZero) {
417
        iFlag += 1;
418
    }
419
    else {
420
        iMaxGrids *= iCtGridPerAxis;
421
    }
422

423
    if (bLenghtYisZero) {
424
        iFlag += 2;
425
    }
426
    else {
427
        iMaxGrids *= iCtGridPerAxis;
428
    }
429

430
    if (bLenghtZisZero) {
431
        iFlag += 4;
432
    }
433
    else {
434
        iMaxGrids *= iCtGridPerAxis;
435
    }
436

437
    unsigned long ulGridsFacets = 10;
438

439
    double fFactorVolumen = 40.0;
440
    double fFactorArea = 10.0;
441

442
    switch (iFlag) {
443
        case 0: {
444
            double fVolumen = fLenghtX * fLenghtY * fLenghtZ;
445

446
            double fVolumenGrid = (fVolumen * ulGridsFacets) / (fFactorVolumen * _ulCtElements);
447

448
            if ((fVolumenGrid * iMaxGrids) < fVolumen) {
449
                fVolumenGrid = fVolumen / (float)iMaxGrids;
450
            }
451

452
            double fLengthGrid = float(pow((float)fVolumenGrid, 1.0F / 3.0F));
453

454
            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
455
            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
456
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);
457

458
        } break;
459
        case 1: {
460
            _ulCtGridsX = 1;  // bLenghtXisZero
461

462
            double fArea = fLenghtY * fLenghtZ;
463

464
            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
465

466
            if ((fAreaGrid * iMaxGrids) < fArea) {
467
                fAreaGrid = fArea / (double)iMaxGrids;
468
            }
469

470
            double fLengthGrid = double(sqrt(fAreaGrid));
471

472
            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
473
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);
474
        } break;
475
        case 2: {
476
            _ulCtGridsY = 1;  // bLenghtYisZero
477

478
            double fArea = fLenghtX * fLenghtZ;
479

480
            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
481

482
            if ((fAreaGrid * iMaxGrids) < fArea) {
483
                fAreaGrid = fArea / (double)iMaxGrids;
484
            }
485

486
            double fLengthGrid = double(sqrt(fAreaGrid));
487

488
            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
489
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);
490
        } break;
491
        case 3: {
492
            _ulCtGridsX = 1;          // bLenghtXisZero
493
            _ulCtGridsY = 1;          // bLenghtYisZero
494
            _ulCtGridsZ = iMaxGrids;  // bLenghtYisZero
495
        } break;
496
        case 4: {
497
            _ulCtGridsZ = 1;  // bLenghtZisZero
498

499
            double fArea = fLenghtX * fLenghtY;
500

501
            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);
502

503
            if ((fAreaGrid * iMaxGrids) < fArea) {
504
                fAreaGrid = fArea / (float)iMaxGrids;
505
            }
506

507
            double fLengthGrid = double(sqrt(fAreaGrid));
508

509
            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
510
            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
511
        } break;
512
        case 5: {
513
            _ulCtGridsX = 1;          // bLenghtXisZero
514
            _ulCtGridsZ = 1;          // bLenghtZisZero
515
            _ulCtGridsY = iMaxGrids;  // bLenghtYisZero
516
        } break;
517
        case 6: {
518
            _ulCtGridsY = 1;          // bLenghtYisZero
519
            _ulCtGridsZ = 1;          // bLenghtZisZero
520
            _ulCtGridsX = iMaxGrids;  // bLenghtYisZero
521
        } break;
522
        case 7: {
523
            _ulCtGridsX = iMaxGrids;  // bLenghtXisZero
524
            _ulCtGridsY = iMaxGrids;  // bLenghtYisZero
525
            _ulCtGridsZ = iMaxGrids;  // bLenghtZisZero
526
        } break;
527
    }
528
}
529

530
void PointsGrid::SearchNearestFromPoint(const Base::Vector3d& rclPt,
531
                                        std::set<unsigned long>& raclInd) const
532
{
533
    raclInd.clear();
534
    Base::BoundBox3d clBB = GetBoundBox();
535

536
    if (clBB.IsInBox(rclPt)) {  // Point lies within
537
        unsigned long ulX {};
538
        unsigned long ulY {};
539
        unsigned long ulZ {};
540
        Position(rclPt, ulX, ulY, ulZ);
541
        unsigned long ulLevel = 0;
542
        while (raclInd.empty()) {
543
            GetHull(ulX, ulY, ulZ, ulLevel++, raclInd);
544
        }
545
        GetHull(ulX, ulY, ulZ, ulLevel, raclInd);
546
    }
547
    else {  // Point outside
548
        Base::BoundBox3d::SIDE tSide = clBB.GetSideFromRay(rclPt, clBB.GetCenter() - rclPt);
549
        switch (tSide) {
550
            case Base::BoundBox3d::RIGHT: {
551
                int nX = 0;
552
                while (raclInd.empty()) {
553
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
554
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
555
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
556
                        }
557
                    }
558
                    nX++;
559
                }
560
                break;
561
            }
562
            case Base::BoundBox3d::LEFT: {
563
                int nX = _ulCtGridsX - 1;
564
                while (raclInd.empty()) {
565
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
566
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
567
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
568
                        }
569
                    }
570
                    nX++;
571
                }
572
                break;
573
            }
574
            case Base::BoundBox3d::TOP: {
575
                int nY = 0;
576
                while (raclInd.empty()) {
577
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
578
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
579
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
580
                        }
581
                    }
582
                    nY++;
583
                }
584
                break;
585
            }
586
            case Base::BoundBox3d::BOTTOM: {
587
                int nY = _ulCtGridsY - 1;
588
                while (raclInd.empty()) {
589
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
590
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
591
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
592
                        }
593
                    }
594
                    nY--;
595
                }
596
                break;
597
            }
598
            case Base::BoundBox3d::BACK: {
599
                int nZ = 0;
600
                while (raclInd.empty()) {
601
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
602
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
603
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
604
                        }
605
                    }
606
                    nZ++;
607
                }
608
                break;
609
            }
610
            case Base::BoundBox3d::FRONT: {
611
                int nZ = _ulCtGridsZ - 1;
612
                while (raclInd.empty()) {
613
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
614
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
615
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
616
                        }
617
                    }
618
                    nZ--;
619
                }
620
                break;
621
            }
622

623
            default:
624
                break;
625
        }
626
    }
627
}
628

629
void PointsGrid::GetHull(unsigned long ulX,
630
                         unsigned long ulY,
631
                         unsigned long ulZ,
632
                         unsigned long ulDistance,
633
                         std::set<unsigned long>& raclInd) const
634
{
635
    int nX1 = std::max<int>(0, int(ulX) - int(ulDistance));
636
    int nY1 = std::max<int>(0, int(ulY) - int(ulDistance));
637
    int nZ1 = std::max<int>(0, int(ulZ) - int(ulDistance));
638
    int nX2 = std::min<int>(int(_ulCtGridsX) - 1, int(ulX) + int(ulDistance));
639
    int nY2 = std::min<int>(int(_ulCtGridsY) - 1, int(ulY) + int(ulDistance));
640
    int nZ2 = std::min<int>(int(_ulCtGridsZ) - 1, int(ulZ) + int(ulDistance));
641

642
    // top plane
643
    for (int i = nX1; i <= nX2; i++) {
644
        for (int j = nY1; j <= nY2; j++) {
645
            GetElements(i, j, nZ1, raclInd);
646
        }
647
    }
648
    // bottom plane
649
    for (int i = nX1; i <= nX2; i++) {
650
        for (int j = nY1; j <= nY2; j++) {
651
            GetElements(i, j, nZ2, raclInd);
652
        }
653
    }
654
    // left plane
655
    for (int i = nY1; i <= nY2; i++) {
656
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
657
            GetElements(nX1, i, j, raclInd);
658
        }
659
    }
660
    // right plane
661
    for (int i = nY1; i <= nY2; i++) {
662
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
663
            GetElements(nX2, i, j, raclInd);
664
        }
665
    }
666
    // front plane
667
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
668
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
669
            GetElements(i, nY1, j, raclInd);
670
        }
671
    }
672
    // back plane
673
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
674
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
675
            GetElements(i, nY2, j, raclInd);
676
        }
677
    }
678
}
679

680
unsigned long PointsGrid::GetElements(unsigned long ulX,
681
                                      unsigned long ulY,
682
                                      unsigned long ulZ,
683
                                      std::set<unsigned long>& raclInd) const
684
{
685
    const std::set<unsigned long>& rclSet = _aulGrid[ulX][ulY][ulZ];
686
    if (!rclSet.empty()) {
687
        raclInd.insert(rclSet.begin(), rclSet.end());
688
        return rclSet.size();
689
    }
690

691
    return 0;
692
}
693

694
void PointsGrid::AddPoint(const Base::Vector3d& rclPt, unsigned long ulPtIndex, float /*fEpsilon*/)
695
{
696
    unsigned long ulX {}, ulY {}, ulZ {};
697
    Pos(Base::Vector3d(rclPt.x, rclPt.y, rclPt.z), ulX, ulY, ulZ);
698
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
699
        _aulGrid[ulX][ulY][ulZ].insert(ulPtIndex);
700
    }
701
}
702

703
void PointsGrid::Validate(const PointKernel& rclPoints)
704
{
705
    if (_pclPoints != &rclPoints) {
706
        Attach(rclPoints);
707
    }
708
    else if (rclPoints.size() != _ulCtElements) {
709
        RebuildGrid();
710
    }
711
}
712

713
void PointsGrid::Validate()
714
{
715
    if (!_pclPoints) {
716
        return;
717
    }
718

719
    if (_pclPoints->size() != _ulCtElements) {
720
        RebuildGrid();
721
    }
722
}
723

724
bool PointsGrid::Verify() const
725
{
726
    if (!_pclPoints) {
727
        return false;  // no point cloud attached
728
    }
729
    if (_pclPoints->size() != _ulCtElements) {
730
        return false;  // not up-to-date
731
    }
732

733
    PointsGridIterator it(*this);
734
    for (it.Init(); it.More(); it.Next()) {
735
        std::vector<unsigned long> aulElements;
736
        it.GetElements(aulElements);
737
        for (unsigned long element : aulElements) {
738
            const Base::Vector3d& cP = _pclPoints->getPoint(element);
739
            if (!it.GetBoundBox().IsInBox(cP)) {
740
                return false;  // point doesn't lie inside the grid element
741
            }
742
        }
743
    }
744

745
    return true;
746
}
747

748
void PointsGrid::RebuildGrid()
749
{
750
    _ulCtElements = _pclPoints->size();
751

752
    InitGrid();
753

754
    // Fill data structure
755

756
    unsigned long i = 0;
757
    for (const auto& pnt : *_pclPoints) {
758
        AddPoint(pnt, i++);
759
    }
760
}
761

762
void PointsGrid::Pos(const Base::Vector3d& rclPoint,
763
                     unsigned long& rulX,
764
                     unsigned long& rulY,
765
                     unsigned long& rulZ) const
766
{
767
    rulX = (unsigned long)((rclPoint.x - _fMinX) / _fGridLenX);
768
    rulY = (unsigned long)((rclPoint.y - _fMinY) / _fGridLenY);
769
    rulZ = (unsigned long)((rclPoint.z - _fMinZ) / _fGridLenZ);
770
}
771

772
unsigned long PointsGrid::FindElements(const Base::Vector3d& rclPoint,
773
                                       std::set<unsigned long>& aulElements) const
774
{
775
    unsigned long ulX {}, ulY {}, ulZ {};
776
    Pos(rclPoint, ulX, ulY, ulZ);
777

778
    // check if the given point is inside the grid structure
779
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
780
        return GetElements(ulX, ulY, ulZ, aulElements);
781
    }
782

783
    return 0;
784
}
785

786
// ----------------------------------------------------------------
787

788
PointsGridIterator::PointsGridIterator(const PointsGrid& rclG)
789
    : _rclGrid(rclG)
790
    , _clPt(0.0F, 0.0F, 0.0F)
791
    , _clDir(0.0F, 0.0F, 0.0F)
792
{}
793

794
bool PointsGridIterator::InitOnRay(const Base::Vector3d& rclPt,
795
                                   const Base::Vector3d& rclDir,
796
                                   float fMaxSearchArea,
797
                                   std::vector<unsigned long>& raulElements)
798
{
799
    bool ret = InitOnRay(rclPt, rclDir, raulElements);
800
    _fMaxSearchArea = fMaxSearchArea;
801
    return ret;
802
}
803

804
bool PointsGridIterator::InitOnRay(const Base::Vector3d& rclPt,
805
                                   const Base::Vector3d& rclDir,
806
                                   std::vector<unsigned long>& raulElements)
807
{
808
    // needed in NextOnRay() to avoid an infinite loop
809
    _cSearchPositions.clear();
810

811
    _fMaxSearchArea = FLOAT_MAX;
812

813
    raulElements.clear();
814

815
    _clPt = rclPt;
816
    _clDir = rclDir;
817
    _bValidRay = false;
818

819
    // point lies within global BB
820
    if (_rclGrid.GetBoundBox().IsInBox(rclPt)) {  // determine the voxel by the starting point
821
        _rclGrid.Position(rclPt, _ulX, _ulY, _ulZ);
822
        raulElements.insert(raulElements.end(),
823
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
824
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
825
        _bValidRay = true;
826
    }
827
    else {  // StartPoint outside
828
        Base::Vector3d cP0, cP1;
829
        if (_rclGrid.GetBoundBox().IntersectWithLine(rclPt,
830
                                                     rclDir,
831
                                                     cP0,
832
                                                     cP1)) {  // determine the next point
833
            if ((cP0 - rclPt).Length() < (cP1 - rclPt).Length()) {
834
                _rclGrid.Position(cP0, _ulX, _ulY, _ulZ);
835
            }
836
            else {
837
                _rclGrid.Position(cP1, _ulX, _ulY, _ulZ);
838
            }
839

840
            raulElements.insert(raulElements.end(),
841
                                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
842
                                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
843
            _bValidRay = true;
844
        }
845
    }
846

847
    return _bValidRay;
848
}
849

850
bool PointsGridIterator::NextOnRay(std::vector<unsigned long>& raulElements)
851
{
852
    if (!_bValidRay) {
853
        return false;  // not initialized or beam exited
854
    }
855

856
    raulElements.clear();
857

858
    Base::Vector3d clIntersectPoint;
859

860
    // Look for the next adjacent BB on the search beam
861
    Base::BoundBox3d::SIDE tSide =
862
        _rclGrid.GetBoundBox(_ulX, _ulY, _ulZ).GetSideFromRay(_clPt, _clDir, clIntersectPoint);
863

864
    // Search area
865
    //
866
    if ((_clPt - clIntersectPoint).Length() > _fMaxSearchArea) {
867
        _bValidRay = false;
868
    }
869
    else {
870
        switch (tSide) {
871
            case Base::BoundBox3d::LEFT:
872
                _ulX--;
873
                break;
874
            case Base::BoundBox3d::RIGHT:
875
                _ulX++;
876
                break;
877
            case Base::BoundBox3d::BOTTOM:
878
                _ulY--;
879
                break;
880
            case Base::BoundBox3d::TOP:
881
                _ulY++;
882
                break;
883
            case Base::BoundBox3d::FRONT:
884
                _ulZ--;
885
                break;
886
            case Base::BoundBox3d::BACK:
887
                _ulZ++;
888
                break;
889

890
            default:
891
            case Base::BoundBox3d::INVALID:
892
                _bValidRay = false;
893
                break;
894
        }
895

896
        GridElement pos(_ulX, _ulY, _ulZ);
897
        if (_cSearchPositions.find(pos) != _cSearchPositions.end()) {
898
            _bValidRay =
899
                false;  // grid element already visited => result from GetSideFromRay invalid
900
        }
901
    }
902

903
    if (_bValidRay && _rclGrid.CheckPos(_ulX, _ulY, _ulZ)) {
904
        GridElement pos(_ulX, _ulY, _ulZ);
905
        _cSearchPositions.insert(pos);
906
        raulElements.insert(raulElements.end(),
907
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
908
                            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end());
909
    }
910
    else {
911
        _bValidRay = false;  // ray exited
912
    }
913

914
    return _bValidRay;
915
}
916