1 /*
2  Copyright (C) 2010-2014 Kristian Duske
3 
4  This file is part of TrenchBroom.
5 
6  TrenchBroom is free software: you can redistribute it and/or modify
7  it under the terms of the GNU General Public License as published by
8  the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  TrenchBroom 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 General Public License for more details.
15 
16  You should have received a copy of the GNU General Public License
17  along with TrenchBroom. If not, see <http://www.gnu.org/licenses/>.
18  */
19 
20 #ifndef TrenchBroom_Octree
21 #define TrenchBroom_Octree
22 
23 #include "CollectionUtils.h"
24 #include "Macros.h"
25 #include "VecMath.h"
26 #include "Exceptions.h"
27 #include "SharedPointer.h"
28 
29 #include <map>
30 #include <vector>
31 
32 namespace TrenchBroom {
33     namespace Model {
34         template <typename F, typename T>
35         class OctreeNode {
36         private:
37             typedef std::vector<T> List;
38 
39             BBox<F,3> m_bounds;
40             F m_minSize;
41             OctreeNode* m_parent;
42             OctreeNode* m_children[8];
43             List m_objects;
44         public:
OctreeNode(const BBox<F,3> & bounds,const F minSize,OctreeNode * parent)45             OctreeNode(const BBox<F,3>& bounds, const F minSize, OctreeNode* parent) :
46             m_bounds(bounds),
47             m_minSize(minSize),
48             m_parent(parent) {
49                 for (size_t i = 0; i < 8; ++i)
50                     m_children[i] = NULL;
51             }
52 
~OctreeNode()53             ~OctreeNode() {
54                 for (size_t i = 0; i < 8; ++i) {
55                     if (m_children[i] != NULL) {
56                         delete m_children[i];
57                         m_children[i] = NULL;
58                     }
59                 }
60             }
61 
contains(const BBox<F,3> & bounds)62             bool contains(const BBox<F,3>& bounds) const {
63                 return m_bounds.contains(bounds);
64             }
65 
containsObject(const BBox<F,3> & bounds,T object)66             bool containsObject(const BBox<F,3>& bounds, T object) const {
67                 assert(contains(bounds));
68                 for (size_t i = 0; i < 8; ++i) {
69                     if (m_children[i] != NULL && m_children[i]->contains(bounds)) {
70                         return m_children[i]->containsObject(bounds, object);
71                     }
72                 }
73 
74                 typename List::const_iterator it = std::find(m_objects.begin(), m_objects.end(), object);
75                 return it != m_objects.end();
76             }
77 
addObject(const BBox<F,3> & bounds,T object)78             OctreeNode* addObject(const BBox<F,3>& bounds, T object) {
79                 assert(contains(bounds));
80 
81                 const Vec3f size = m_bounds.size();
82                 if (size.x() > m_minSize || size.y() > m_minSize || size.z() > m_minSize) {
83                     for (size_t i = 0; i < 8; ++i) {
84                         if (m_children[i] != NULL && m_children[i]->contains(bounds)) {
85                             return m_children[i]->addObject(bounds, object);
86                         } else {
87                             const BBox<F,3> childBounds = octant(i);
88                             if (childBounds.contains(bounds)) {
89                                 OctreeNode<F,T>* child = new OctreeNode<F,T>(childBounds, m_minSize, this);
90                                 try {
91                                     OctreeNode* result = child->addObject(bounds, object);
92                                     m_children[i] = child;
93                                     return result;
94                                 } catch (...) {
95                                     delete child;
96                                     throw;
97                                 }
98                             }
99                         }
100                     }
101                 }
102 
103                 assert(!VectorUtils::contains(m_objects, object));
104                 m_objects.push_back(object);
105                 return this;
106             }
107 
removeObject(T object)108             bool removeObject(T object) {
109                 typename List::iterator it = std::find(m_objects.begin(), m_objects.end(), object);
110                 if (it == m_objects.end())
111                     return false;
112                 m_objects.erase(it);
113                 return true;
114             }
115 
findContaining(const BBox<F,3> & bounds)116             OctreeNode* findContaining(const BBox<F,3>& bounds) {
117                 if (contains(bounds))
118                     return this;
119                 if (m_parent == NULL)
120                     return NULL;
121                 return m_parent->findContaining(bounds);
122             }
123 
findObjects(const Ray<F,3> & ray,List & result)124             void findObjects(const Ray<F,3>& ray, List& result) const {
125                 const F distance = m_bounds.intersectWithRay(ray);
126                 if (Math::isnan(distance))
127                     return;
128 
129                 for (size_t i = 0; i < 8; ++i)
130                     if (m_children[i] != NULL)
131                         m_children[i]->findObjects(ray, result);
132                 result.insert(result.end(), m_objects.begin(), m_objects.end());
133             }
134 
findObjects(const Vec<F,3> & point,List & result)135             void findObjects(const Vec<F,3>& point, List& result) const {
136                 if (!m_bounds.contains(point))
137                     return;
138 
139                 for (size_t i = 0; i < 8; ++i)
140                     if (m_children[i] != NULL)
141                         m_children[i]->findObjects(point, result);
142                 result.insert(result.end(), m_objects.begin(), m_objects.end());
143             }
144         private:
octant(const size_t index)145             BBox<F,3> octant(const size_t index) const {
146                 const Vec3f& min = m_bounds.min;
147                 const Vec3f& max = m_bounds.max;
148                 const Vec3f mid = (min + max) / 2.0f;
149                 switch (index) {
150                     case 0: // xyz +++
151                         return BBox<F,3>(mid, max);
152                     case 1: // xyz -++
153                         return BBox<F,3>(Vec3f(min.x(), mid.y(), mid.z()),
154                                       Vec3f(mid.x(), max.y(), max.z()));
155                     case 2: // xyz +-+
156                         return BBox<F,3>(Vec3f(mid.x(), min.y(), mid.z()),
157                                       Vec3f(max.x(), mid.y(), max.z()));
158                     case 3: // xyz --+
159                         return BBox<F,3>(Vec3f(min.x(), min.y(), mid.z()),
160                                       Vec3f(mid.x(), mid.y(), max.z()));
161                     case 4: // xyz ++-
162                         return BBox<F,3>(Vec3f(mid.x(), mid.y(), min.z()),
163                                       Vec3f(max.x(), max.y(), mid.z()));
164                     case 5: // xyz -+-
165                         return BBox<F,3>(Vec3f(min.x(), mid.y(), min.z()),
166                                       Vec3f(mid.x(), max.y(), mid.z()));
167                     case 6: // xyz +--
168                         return BBox<F,3>(Vec3f(mid.x(), min.y(), min.z()),
169                                       Vec3f(max.x(), mid.y(), mid.z()));
170                     case 7: // xyz ---
171                         return BBox<F,3>(Vec3f(min.x(), min.y(), min.z()),
172                                       Vec3f(mid.x(), mid.y(), mid.z()));
173                     default:
174                         assert(false);
175                         return BBox<F,3>();
176                 }
177             }
178         };
179 
180         template <typename F, typename T>
181         class Octree {
182         public:
183             typedef std::vector<T> List;
184         private:
185             typedef std::map<T, OctreeNode<F,T>*> ObjectMap;
186             BBox<F,3> m_bounds;
187             OctreeNode<F,T>* m_root;
188             ObjectMap m_objectMap;
189         public:
Octree(const BBox<F,3> & bounds,const F minSize)190             Octree(const BBox<F,3>& bounds, const F minSize) :
191             m_bounds(bounds),
192             m_root(new OctreeNode<F,T>(bounds, minSize, NULL)) {}
193 
~Octree()194             ~Octree() {
195                 delete m_root;
196             }
197 
bounds()198             const BBox<F,3>& bounds() const {
199                 return m_bounds;
200             }
201 
addObject(const BBox<F,3> & bounds,T object)202             void addObject(const BBox<F,3>& bounds, T object) {
203                 if (!m_root->contains(bounds))
204                     throw OctreeException("Object is too large for this octree");
205 
206                 OctreeNode<F,T>* node = m_root->addObject(bounds, object);
207                 if (node == NULL)
208                     throw OctreeException("Unknown error when inserting into octree");
209                 assertResult(MapUtils::insertOrFail(m_objectMap, object, node));
210             }
211 
removeObject(T object)212             void removeObject(T object) {
213                 typename ObjectMap::iterator it = m_objectMap.find(object);
214                 if (it == m_objectMap.end())
215                     throw OctreeException("Cannot find object in octree");
216 
217                 OctreeNode<F,T>* node = it->second;
218                 if (!node->removeObject(object))
219                     throw OctreeException("Cannot find object in octree");
220                 m_objectMap.erase(it);
221             }
222 
updateObject(const BBox<F,3> & bounds,T object)223             void updateObject(const BBox<F,3>& bounds, T object) {
224                 typename ObjectMap::iterator it = m_objectMap.find(object);
225                 if (it == m_objectMap.end())
226                     throw OctreeException("Cannot find object in octree");
227 
228                 OctreeNode<F,T>* oldNode = it->second;
229                 if (!oldNode->removeObject(object))
230                     throw OctreeException("Cannot find object in octree");
231 
232                 OctreeNode<F,T>* newAncestor = oldNode->findContaining(bounds);
233                 if (newAncestor == NULL)
234                     throw OctreeException("Cannot find new ancestor node in octree");
235                 OctreeNode<F,T>* newParent = newAncestor->addObject(bounds, object);
236                 if (newParent == NULL)
237                     throw OctreeException("Unknown error when inserting into octree");
238                 MapUtils::insertOrReplace(m_objectMap, object, newParent);
239             }
240 
containsObject(const BBox<F,3> & bounds,T object)241             bool containsObject(const BBox<F,3>& bounds, T object) const {
242                 if (!m_root->contains(bounds))
243                     return false;
244                 return m_root->containsObject(bounds, object);
245             }
246 
findObjects(const Ray<F,3> & ray)247             List findObjects(const Ray<F,3>& ray) const {
248                 List result;
249                 m_root->findObjects(ray, result);
250                 return result;
251             }
252 
findObjects(const Vec<F,3> & point)253             List findObjects(const Vec<F,3>& point) const {
254                 List result;
255                 m_root->findObjects(point, result);
256                 return result;
257             }
258         };
259     }
260 }
261 
262 #endif /* defined(TrenchBroom_Octree) */
263