2 * PositionedOctree - define a spatial octree containing Positioned items
3 * arranged by their global cartesian position.
6 // Written by James Turner, started 2012.
8 // Copyright (C) 2012 James Turner
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // General Public License for more details.
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
28 #include "PositionedOctree.hxx"
29 #include "positioned.hxx"
32 #include <algorithm> // for sort
33 #include <cstring> // for memset
36 #include <boost/foreach.hpp>
38 #include <simgear/debug/logstream.hxx>
39 #include <simgear/structure/exception.hxx>
47 Node* global_spatialOctree = NULL;
49 Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent) :
55 void Leaf::visit(const SGVec3d& aPos, double aCutoff,
56 FGPositioned::Filter* aFilter,
57 FindNearestResults& aResults, FindNearestPQueue&)
59 int previousResultsSize = aResults.size();
61 NavDataCache* cache = NavDataCache::instance();
65 ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
66 ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
68 for (; it != end; ++it) {
69 FGPositioned* p = cache->loadById(it->second);
70 if (!intersects(_box, p->cart())) {
71 // see http://code.google.com/p/flightgear-bugs/issues/detail?id=905
73 SG_LOG(SG_GENERAL, SG_WARN, "XXXXXXXXX bad spatial index for " << it->second
74 << "; " << p->ident() << " of type " <<
75 FGPositioned::nameForType(p->type()));
76 SG_LOG(SG_GENERAL, SG_WARN, "\tgeodetic location:" << p->geod());
77 SG_LOG(SG_GENERAL, SG_WARN, "\tcartesian location:" << p->cart());
79 SG_LOG(SG_GENERAL, SG_WARN, "leaf box:" <<
80 _box.getMin() << " x " << _box.getMax());
82 throw sg_exception("Bad spatial index data");
86 double d = dist(aPos, p->cart());
91 if (aFilter && !aFilter->pass(p)) {
96 aResults.push_back(OrderedPositioned(p, d));
99 if (addedCount == 0) {
103 // keep aResults sorted
104 // sort the new items, usually just one or two items
105 std::sort(aResults.begin() + previousResultsSize, aResults.end());
107 // merge the two sorted ranges together - in linear time
108 std::inplace_merge(aResults.begin(),
109 aResults.begin() + previousResultsSize, aResults.end());
112 void Leaf::insertChild(FGPositioned::Type ty, PositionedID id)
114 assert(childrenLoaded);
115 children.insert(children.end(), TypedPositioned(ty, id));
118 void Leaf::loadChildren()
120 if (childrenLoaded) {
124 NavDataCache* cache = NavDataCache::instance();
125 BOOST_FOREACH(TypedPositioned tp, cache->getOctreeLeafChildren(guid())) {
126 children.insert(children.end(), tp);
127 } // of leaf members iteration
129 childrenLoaded = true;
132 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
134 childrenLoaded(false)
136 memset(children, 0, sizeof(Node*) * 8);
139 void Branch::visit(const SGVec3d& aPos, double aCutoff,
140 FGPositioned::Filter*,
141 FindNearestResults&, FindNearestPQueue& aQ)
144 for (unsigned int i=0; i<8; ++i) {
149 double d = children[i]->distToNearest(aPos);
151 continue; // exceeded cutoff
154 aQ.push(Ordered<Node*>(children[i], d));
155 } // of child iteration
158 Node* Branch::childForPos(const SGVec3d& aCart) const
160 assert(contains(aCart));
163 SGVec3d center(_box.getCenter());
164 // tests must match indices in SGbox::getCorner
165 if (aCart.x() < center.x()) {
169 if (aCart.y() < center.y()) {
173 if (aCart.z() < center.z()) {
177 return childAtIndex(childIndex);
180 Node* Branch::childAtIndex(int childIndex) const
182 Node* child = children[childIndex];
183 if (!child) { // lazy building of children
184 SGBoxd cb(boxForChild(childIndex));
185 double d2 = dot(cb.getSize(), cb.getSize());
187 assert(((_ident << 3) >> 3) == _ident);
189 // child index is 0..7, so 3-bits is sufficient, and hence we can
190 // pack 20 levels of octree into a int64, which is plenty
191 int64_t childIdent = (_ident << 3) | childIndex;
193 if (d2 < LEAF_SIZE_SQR) {
194 child = new Leaf(cb, childIdent);
196 child = new Branch(cb, childIdent);
199 children[childIndex] = child;
201 if (childrenLoaded) {
202 // childrenLoad is done, so we're defining a new node - add it to the
204 NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
208 return children[childIndex];
211 void Branch::loadChildren() const
213 if (childrenLoaded) {
217 int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
218 for (int i=0; i<8; ++i) {
219 if ((1 << i) & childrenMask) {
220 childAtIndex(i); // accessing will create!
222 } // of child index iteration
224 // set this after creating the child nodes, so the cache update logic
225 // in childAtIndex knows any future created children need to be added.
226 childrenLoaded = true;
229 int Branch::childMask() const
232 for (int i=0; i<8; ++i) {
241 void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
244 FindNearestPQueue pq;
245 FindNearestResults results;
246 pq.push(Ordered<Node*>(global_spatialOctree, 0));
247 double cut = aCutoffM;
249 while (!pq.empty()) {
250 if (!results.empty()) {
251 // terminate the search if we have sufficent results, and we are
252 // sure no node still on the queue contains a closer match
253 double furthestResultOrder = results.back().order();
254 // std::cout << "furthest result:" << furthestResultOrder << ", top node order:" << pq.top().order() << std::endl;
255 if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
260 Node* nd = pq.top().get();
263 // std::cout << "visiting:" << std::oct << nd->guid() << "(" << std::dec << nd->guid() << ")" << std::endl;
264 nd->visit(aPos, cut, aFilter, results, pq);
265 } // of queue iteration
267 // depending on leaf population, we may have (slighty) more results
269 unsigned int numResults = std::min((unsigned int) results.size(), aN);
271 aResults.resize(numResults);
272 for (unsigned int r=0; r<numResults; ++r) {
273 aResults[r] = results[r].get();
277 void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
280 FindNearestPQueue pq;
281 FindNearestResults results;
282 pq.push(Ordered<Node*>(global_spatialOctree, 0));
283 double rng = aRangeM;
285 while (!pq.empty()) {
286 Node* nd = pq.top().get();
289 nd->visit(aPos, rng, aFilter, results, pq);
290 } // of queue iteration
292 unsigned int numResults = results.size();
294 aResults.resize(numResults);
295 for (unsigned int r=0; r<numResults; ++r) {
296 aResults[r] = results[r].get();
300 } // of namespace Octree
302 } // of namespace flightgear