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 void Leaf::addPolyLine(PolyLineRef aLine)
119 lines.push_back(aLine);
122 void Leaf::visitForLines(const SGVec3d& aPos, double aCutoff,
123 PolyLineList& aLines,
124 FindLinesDeque& aQ) const
126 aLines.insert(aLines.end(), lines.begin(), lines.end());
129 ///////////////////////////////////////////////////////////////////////////////
131 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
133 childrenLoaded(false)
135 memset(children, 0, sizeof(Node*) * 8);
138 void Branch::visit(const SGVec3d& aPos, double aCutoff,
139 FGPositioned::Filter*,
140 FindNearestResults&, FindNearestPQueue& aQ)
143 for (unsigned int i=0; i<8; ++i) {
148 double d = children[i]->distToNearest(aPos);
150 continue; // exceeded cutoff
153 aQ.push(Ordered<Node*>(children[i], d));
154 } // of child iteration
157 void Branch::visitForLines(const SGVec3d& aPos, double aCutoff,
158 PolyLineList& aLines,
159 FindLinesDeque& aQ) const
161 for (unsigned int i=0; i<8; ++i) {
166 double d = children[i]->distToNearest(aPos);
168 continue; // exceeded cutoff
171 aQ.push_back(children[i]);
172 } // of child iteration
175 Node* Branch::childForPos(const SGVec3d& aCart) const
177 assert(contains(aCart));
180 SGVec3d center(_box.getCenter());
181 // tests must match indices in SGbox::getCorner
182 if (aCart.x() < center.x()) {
186 if (aCart.y() < center.y()) {
190 if (aCart.z() < center.z()) {
194 return childAtIndex(childIndex);
197 Node* Branch::childAtIndex(int childIndex) const
199 Node* child = children[childIndex];
200 if (!child) { // lazy building of children
201 SGBoxd cb(boxForChild(childIndex));
202 double d2 = dot(cb.getSize(), cb.getSize());
204 assert(((_ident << 3) >> 3) == _ident);
206 // child index is 0..7, so 3-bits is sufficient, and hence we can
207 // pack 20 levels of octree into a int64, which is plenty
208 int64_t childIdent = (_ident << 3) | childIndex;
210 if (d2 < LEAF_SIZE_SQR) {
211 child = new Leaf(cb, childIdent);
213 child = new Branch(cb, childIdent);
216 children[childIndex] = child;
218 if (childrenLoaded) {
219 // childrenLoad is done, so we're defining a new node - add it to the
221 NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
225 return children[childIndex];
228 void Branch::loadChildren() const
230 if (childrenLoaded) {
234 int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
235 for (int i=0; i<8; ++i) {
236 if ((1 << i) & childrenMask) {
237 childAtIndex(i); // accessing will create!
239 } // of child index iteration
241 // set this after creating the child nodes, so the cache update logic
242 // in childAtIndex knows any future created children need to be added.
243 childrenLoaded = true;
246 int Branch::childMask() const
249 for (int i=0; i<8; ++i) {
258 bool findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
261 FindNearestPQueue pq;
262 FindNearestResults results;
263 pq.push(Ordered<Node*>(global_spatialOctree, 0));
264 double cut = aCutoffM;
269 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
270 if (!results.empty()) {
271 // terminate the search if we have sufficent results, and we are
272 // sure no node still on the queue contains a closer match
273 double furthestResultOrder = results.back().order();
274 if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
275 // clear the PQ to mark this has 'full results' instead of partial
276 pq = FindNearestPQueue();
281 Node* nd = pq.top().get();
284 nd->visit(aPos, cut, aFilter, results, pq);
285 } // of queue iteration
287 // depending on leaf population, we may have (slighty) more results
289 unsigned int numResults = std::min((unsigned int) results.size(), aN);
291 aResults.resize(numResults);
292 for (unsigned int r=0; r<numResults; ++r) {
293 aResults[r] = results[r].get();
299 bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
302 FindNearestPQueue pq;
303 FindNearestResults results;
304 pq.push(Ordered<Node*>(global_spatialOctree, 0));
305 double rng = aRangeM;
310 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
311 Node* nd = pq.top().get();
314 nd->visit(aPos, rng, aFilter, results, pq);
315 } // of queue iteration
317 unsigned int numResults = results.size();
319 aResults.resize(numResults);
320 for (unsigned int r=0; r<numResults; ++r) {
321 aResults[r] = results[r].get();
327 } // of namespace Octree
329 } // of namespace flightgear