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