- int bucketIndex = aPos->bucket().gen_index();
- SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex);
- if (it != global_spatialIndex.end()) {
- return it;
- }
-
- // create a new BucketEntry
- return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry()));
+ aResults.clear();
+ FindNearestPQueue pq;
+ FindNearestResults results;
+ pq.push(Ordered<Node*>(global_spatialOctree, 0));
+ double cut = aCutoffM * aCutoffM;
+
+ while (!pq.empty()) {
+ if (!results.empty()) {
+ // terminate the search if we have sufficent results, and we are
+ // sure no node still on the queue contains a closer match
+ double furthestResultOrder = results.back().order();
+ if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
+ break;
+ }
+ }
+
+ Node* nd = pq.top().get();
+ pq.pop();
+
+ nd->visit(aPos, cut, aFilter, results, pq);
+ } // of queue iteration
+
+ // depending on leaf population, we may have (slighty) more results
+ // than requested
+ unsigned int numResults = std::min((unsigned int) results.size(), aN);
+ // copy results out
+ aResults.resize(numResults);
+ for (unsigned int r=0; r<numResults; ++r) {
+ aResults[r] = results[r].get();
+ }
+}
+
+void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
+{
+ aResults.clear();
+ FindNearestPQueue pq;
+ FindNearestResults results;
+ pq.push(Ordered<Node*>(global_spatialOctree, 0));
+ double rng = aRangeM * aRangeM;
+
+ while (!pq.empty()) {
+ Node* nd = pq.top().get();
+ pq.pop();
+
+ nd->visit(aPos, rng, aFilter, results, pq);
+ } // of queue iteration
+
+ unsigned int numResults = results.size();
+ // copy results out
+ aResults.resize(numResults);
+ for (unsigned int r=0; r<numResults; ++r) {
+ aResults[r] = results[r].get();
+ }