FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2011 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
#ifdef _MSC_VER
25
#pragma warning(disable : 4396)
26
#endif
27

28
#include "KDTree.h"
29
#include <kdtree++/kdtree.hpp>
30

31

32
using namespace MeshCore;
33

34
struct Point3d
35
{
36
    using value_type = float;
37

38
    Point3d(const Base::Vector3f& f, PointIndex i)
39
        : p(f)
40
        , i(i)
41
    {}
42

43
    Point3d(const Point3d& pnt) = default;
44

45
    Point3d(Point3d&& pnt)
46
        : p(pnt.p)
47
        , i(pnt.i)
48
    {}
49

50
    ~Point3d() = default;
51

52
    inline value_type operator[](const int N) const
53
    {
54
        return p[N];
55
    }
56

57
    inline bool operator==(const Point3d& other) const
58
    {
59
        return (this->p) == (other.p);
60
    }
61

62
    inline bool operator!=(const Point3d& other) const
63
    {
64
        return (this->p) != (other.p);
65
    }
66

67
    inline Point3d& operator=(const Point3d& other) = default;
68

69
    inline Point3d& operator=(Point3d&& other)
70
    {
71
        this->p = other.p;
72
        this->i = other.i;
73
        return *this;
74
    }
75

76
    Base::Vector3f p;
77
    PointIndex i;
78
};
79

80
using MyKDTree = KDTree::KDTree<3, Point3d>;
81

82
class MeshKDTree::Private
83
{
84
public:
85
    MyKDTree kd_tree;
86
};
87

88
MeshKDTree::MeshKDTree()
89
    : d(new Private)
90
{}
91

92
MeshKDTree::MeshKDTree(const std::vector<Base::Vector3f>& points)
93
    : d(new Private)
94
{
95
    PointIndex index = 0;
96
    for (auto it : points) {
97
        d->kd_tree.insert(Point3d(it, index++));
98
    }
99
}
100

101
MeshKDTree::MeshKDTree(const MeshPointArray& points)
102
    : d(new Private)
103
{
104
    PointIndex index = 0;
105
    for (const auto& it : points) {
106
        d->kd_tree.insert(Point3d(it, index++));
107
    }
108
}
109

110
MeshKDTree::~MeshKDTree()
111
{
112
    delete d;
113
}
114

115
void MeshKDTree::AddPoint(const Base::Vector3f& point)
116
{
117
    PointIndex index = d->kd_tree.size();
118
    d->kd_tree.insert(Point3d(point, index));
119
}
120

121
void MeshKDTree::AddPoints(const std::vector<Base::Vector3f>& points)
122
{
123
    PointIndex index = d->kd_tree.size();
124
    for (auto it : points) {
125
        d->kd_tree.insert(Point3d(it, index++));
126
    }
127
}
128

129
void MeshKDTree::AddPoints(const MeshPointArray& points)
130
{
131
    PointIndex index = d->kd_tree.size();
132
    for (const auto& it : points) {
133
        d->kd_tree.insert(Point3d(it, index++));
134
    }
135
}
136

137
bool MeshKDTree::IsEmpty() const
138
{
139
    return d->kd_tree.empty();
140
}
141

142
void MeshKDTree::Clear()
143
{
144
    d->kd_tree.clear();
145
}
146

147
void MeshKDTree::Optimize()
148
{
149
    d->kd_tree.optimize();
150
}
151

152
PointIndex MeshKDTree::FindNearest(const Base::Vector3f& p, Base::Vector3f& n, float& dist) const
153
{
154
    std::pair<MyKDTree::const_iterator, MyKDTree::distance_type> it =
155
        d->kd_tree.find_nearest(Point3d(p, 0));
156
    if (it.first == d->kd_tree.end()) {
157
        return POINT_INDEX_MAX;
158
    }
159
    PointIndex index = it.first->i;
160
    n = it.first->p;
161
    dist = it.second;
162
    return index;
163
}
164

165
PointIndex MeshKDTree::FindNearest(const Base::Vector3f& p,
166
                                   float max_dist,
167
                                   Base::Vector3f& n,
168
                                   float& dist) const
169
{
170
    std::pair<MyKDTree::const_iterator, MyKDTree::distance_type> it =
171
        d->kd_tree.find_nearest(Point3d(p, 0), max_dist);
172
    if (it.first == d->kd_tree.end()) {
173
        return POINT_INDEX_MAX;
174
    }
175
    PointIndex index = it.first->i;
176
    n = it.first->p;
177
    dist = it.second;
178
    return index;
179
}
180

181
PointIndex MeshKDTree::FindExact(const Base::Vector3f& p) const
182
{
183
    MyKDTree::const_iterator it = d->kd_tree.find_exact(Point3d(p, 0));
184
    if (it == d->kd_tree.end()) {
185
        return POINT_INDEX_MAX;
186
    }
187
    PointIndex index = it->i;
188
    return index;
189
}
190

191
void MeshKDTree::FindInRange(const Base::Vector3f& p,
192
                             float range,
193
                             std::vector<PointIndex>& indices) const
194
{
195
    std::vector<Point3d> v;
196
    d->kd_tree.find_within_range(Point3d(p, 0), range, std::back_inserter(v));
197
    indices.reserve(v.size());
198
    for (const auto& it : v) {
199
        indices.push_back(it.i);
200
    }
201
}
202