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>
40 #include <simgear/timing/timestamp.hxx>
48 Node* global_spatialOctree = NULL;
50 Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent) :
56 void Leaf::visit(const SGVec3d& aPos, double aCutoff,
57 FGPositioned::Filter* aFilter,
58 FindNearestResults& aResults, FindNearestPQueue&)
60 int previousResultsSize = aResults.size();
62 NavDataCache* cache = NavDataCache::instance();
66 ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
67 ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
69 for (; it != end; ++it) {
70 FGPositioned* p = cache->loadById(it->second);
71 double d = dist(aPos, p->cart());
76 if (aFilter && !aFilter->pass(p)) {
81 aResults.push_back(OrderedPositioned(p, d));
84 if (addedCount == 0) {
88 // keep aResults sorted
89 // sort the new items, usually just one or two items
90 std::sort(aResults.begin() + previousResultsSize, aResults.end());
92 // merge the two sorted ranges together - in linear time
93 std::inplace_merge(aResults.begin(),
94 aResults.begin() + previousResultsSize, aResults.end());
97 void Leaf::insertChild(FGPositioned::Type ty, PositionedID id)
99 assert(childrenLoaded);
100 children.insert(children.end(), TypedPositioned(ty, id));
103 void Leaf::loadChildren()
105 if (childrenLoaded) {
109 NavDataCache* cache = NavDataCache::instance();
110 BOOST_FOREACH(TypedPositioned tp, cache->getOctreeLeafChildren(guid())) {
111 children.insert(children.end(), tp);
112 } // of leaf members iteration
114 childrenLoaded = true;
117 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
119 childrenLoaded(false)
121 memset(children, 0, sizeof(Node*) * 8);
124 void Branch::visit(const SGVec3d& aPos, double aCutoff,
125 FGPositioned::Filter*,
126 FindNearestResults&, FindNearestPQueue& aQ)
129 for (unsigned int i=0; i<8; ++i) {
134 double d = children[i]->distToNearest(aPos);
136 continue; // exceeded cutoff
139 aQ.push(Ordered<Node*>(children[i], d));
140 } // of child iteration
143 Node* Branch::childForPos(const SGVec3d& aCart) const
145 assert(contains(aCart));
148 SGVec3d center(_box.getCenter());
149 // tests must match indices in SGbox::getCorner
150 if (aCart.x() < center.x()) {
154 if (aCart.y() < center.y()) {
158 if (aCart.z() < center.z()) {
162 return childAtIndex(childIndex);
165 Node* Branch::childAtIndex(int childIndex) const
167 Node* child = children[childIndex];
168 if (!child) { // lazy building of children
169 SGBoxd cb(boxForChild(childIndex));
170 double d2 = dot(cb.getSize(), cb.getSize());
172 assert(((_ident << 3) >> 3) == _ident);
174 // child index is 0..7, so 3-bits is sufficient, and hence we can
175 // pack 20 levels of octree into a int64, which is plenty
176 int64_t childIdent = (_ident << 3) | childIndex;
178 if (d2 < LEAF_SIZE_SQR) {
179 child = new Leaf(cb, childIdent);
181 child = new Branch(cb, childIdent);
184 children[childIndex] = child;
186 if (childrenLoaded) {
187 // childrenLoad is done, so we're defining a new node - add it to the
189 NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
193 return children[childIndex];
196 void Branch::loadChildren() const
198 if (childrenLoaded) {
202 int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
203 for (int i=0; i<8; ++i) {
204 if ((1 << i) & childrenMask) {
205 childAtIndex(i); // accessing will create!
207 } // of child index iteration
209 // set this after creating the child nodes, so the cache update logic
210 // in childAtIndex knows any future created children need to be added.
211 childrenLoaded = true;
214 int Branch::childMask() const
217 for (int i=0; i<8; ++i) {
226 bool findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults, int aCutoffMsec)
229 FindNearestPQueue pq;
230 FindNearestResults results;
231 pq.push(Ordered<Node*>(global_spatialOctree, 0));
232 double cut = aCutoffM;
237 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
238 if (!results.empty()) {
239 // terminate the search if we have sufficent results, and we are
240 // sure no node still on the queue contains a closer match
241 double furthestResultOrder = results.back().order();
242 if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
243 // clear the PQ to mark this has 'full results' instead of partial
244 pq = FindNearestPQueue();
249 Node* nd = pq.top().get();
252 nd->visit(aPos, cut, aFilter, results, pq);
253 } // of queue iteration
255 // depending on leaf population, we may have (slighty) more results
257 unsigned int numResults = std::min((unsigned int) results.size(), aN);
259 aResults.resize(numResults);
260 for (unsigned int r=0; r<numResults; ++r) {
261 aResults[r] = results[r].get();
267 bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults, int aCutoffMsec)
270 FindNearestPQueue pq;
271 FindNearestResults results;
272 pq.push(Ordered<Node*>(global_spatialOctree, 0));
273 double rng = aRangeM;
278 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
279 Node* nd = pq.top().get();
282 nd->visit(aPos, rng, aFilter, results, pq);
283 } // of queue iteration
285 unsigned int numResults = results.size();
287 aResults.resize(numResults);
288 for (unsigned int r=0; r<numResults; ++r) {
289 aResults[r] = results[r].get();
295 } // of namespace Octree
297 } // of namespace flightgear