FlightGear next
PositionedOctree.cxx
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: (C) 2012 James Turner <james@flightgear.org>
3 * SPDX_FileComment: define a spatial octree containing Positioned items
4 * SPDX-License-Identifier: GPL-2.0-or-later
5 */
6
7#include "config.h"
8
10#include "positioned.hxx"
11
12#include <cassert>
13#include <algorithm> // for sort
14#include <cstring> // for memset
15#include <iostream>
16
17#include <simgear/debug/logstream.hxx>
18#include <simgear/structure/exception.hxx>
19#include <simgear/timing/timestamp.hxx>
20
21#include "PolyLine.hxx"
22
23namespace flightgear
24{
25
26namespace Octree
27{
28
29static std::unique_ptr<Node> global_spatialOctree;
30static std::unique_ptr<Node> global_transientOctree;
31
32double RADIUS_EARTH_M = 7000 * 1000.0; // 7000km is plenty
33
35{
37 SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M);
38 global_transientOctree.reset(new Octree::Branch(SGBox<double>(-earthExtent, earthExtent), 1, false));
39 }
40
41 return global_transientOctree.get();
42}
43
45{
47 SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M);
48 global_spatialOctree.reset(new Octree::Branch(SGBox<double>(-earthExtent, earthExtent), 1, true));
49 }
50
51 return global_spatialOctree.get();
52}
53
54Node::Node(const SGBoxd& aBox, int64_t aIdent, bool persistent) : _ident(aIdent),
55 _persistent(persistent),
56 _box(aBox)
57{
58}
59
61{
62}
63
65{
66 lines.push_back(aLine);
67}
68
69void Node::visitForLines(const SGVec3d& aPos, double aCutoff,
70 PolyLineList& aLines,
71 FindLinesDeque& aQ) const
72{
73 SG_UNUSED(aPos);
74 SG_UNUSED(aCutoff);
75
76 aLines.insert(aLines.end(), lines.begin(), lines.end());
77}
78
79Node *Node::findNodeForBox(const SGBoxd&) const
80{
81 return const_cast<Node*>(this);
82}
83
84Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent, bool persistent) : Node(aBox, aIdent, persistent)
85{
86 if (!persistent) {
87 _childrenLoaded = true;
88 }
89}
90
91void Leaf::visit(const SGVec3d& aPos, double aCutoff,
92 FGPositioned::Filter* aFilter,
94{
95 int previousResultsSize = aResults.size();
96 int addedCount = 0;
98
99 loadChildren();
100
101 ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
102 ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
103
104 for (; it != end; ++it) {
105 FGPositioned* p = cache->loadById(it->second);
106 double d = dist(aPos, p->cart());
107 if (d > aCutoff) {
108 continue;
109 }
110
111 if (aFilter && !aFilter->pass(p)) {
112 continue;
113 }
114
115 ++addedCount;
116 aResults.push_back(OrderedPositioned(p, d));
117 }
118
119 if (addedCount == 0) {
120 return;
121 }
122
123 // keep aResults sorted
124 // sort the new items, usually just one or two items
125 std::sort(aResults.begin() + previousResultsSize, aResults.end());
126
127 // merge the two sorted ranges together - in linear time
128 std::inplace_merge(aResults.begin(),
129 aResults.begin() + previousResultsSize, aResults.end());
130}
131
133{
134 assert(_childrenLoaded);
135 children.insert(children.end(), TypedPositioned(ty, id));
136}
137
139{
140 // only for use with transient items, i.e negative IDs
141 assert(id < 0);
142
143 auto it = std::find_if(children.begin(), children.end(), [id](const TypedPositioned& p) {
144 return p.second == id;
145 });
146
147 if (it != children.end()) {
148 children.erase(it);
149 }
150}
151
152void Leaf::loadChildren()
153{
154 if (_childrenLoaded) {
155 return;
156 }
157
159 for (const auto& tp : cache->getOctreeLeafChildren(guid())) {
160 // REVIEW: Memory Leak - 1,728 bytes in 36 blocks are still reachable
161 children.insert(children.end(), tp);
162 } // of leaf members iteration
163
164 _childrenLoaded = true;
165}
166
168
169Branch::Branch(const SGBoxd& aBox, int64_t aIdent, bool persistent) : Node(aBox, aIdent, persistent)
170{
171 _children.fill(nullptr);
172 if (!_persistent) {
173 _childrenLoaded = true;
174 }
175}
176
177void Branch::visit(const SGVec3d& aPos, double aCutoff,
180{
181 loadChildren();
182 for (unsigned int i=0; i<8; ++i) {
183 if (!_children[i]) {
184 continue;
185 }
186
187 double d = _children[i]->distToNearest(aPos);
188 if (d > aCutoff) {
189 continue; // exceeded cutoff
190 }
191
192 aQ.push(Ordered<Node*>(_children[i], d));
193 } // of child iteration
194}
195
196void Branch::visitForLines(const SGVec3d& aPos, double aCutoff,
197 PolyLineList& aLines,
198 FindLinesDeque& aQ) const
199{
200 // add our own lines, easy
201 Node::visitForLines(aPos, aCutoff, aLines, aQ);
202
203 for (unsigned int i=0; i<8; ++i) {
204 if (!_children[i]) {
205 continue;
206 }
207
208 double d = _children[i]->distToNearest(aPos);
209 if (d > aCutoff) {
210 continue; // exceeded cutoff
211 }
212
213 aQ.push_back(_children[i]);
214 } // of child iteration
215}
216
217static bool boxContainsBox(const SGBoxd& a, const SGBoxd& b)
218{
219 const SGVec3d aMin(a.getMin()),
220 aMax(a.getMax()),
221 bMin(b.getMin()),
222 bMax(b.getMax());
223 for (int i=0; i<3; ++i) {
224 if ((bMin[i] < aMin[i]) || (bMax[i] > aMax[i])) return false;
225 }
226
227 return true;
228}
229
230Node *Branch::findNodeForBox(const SGBoxd &box) const
231{
232 // do this so childAtIndex sees consistent state of
233 // children[] and loaded flag.
234 loadChildren();
235
236 for (unsigned int i=0; i<8; ++i) {
237 const SGBoxd childBox(boxForChild(i));
238 if (boxContainsBox(childBox, box)) {
239 return childAtIndex(i)->findNodeForBox(box);
240 }
241 }
242
243 return Node::findNodeForBox(box);
244}
245
246Node* Branch::childForPos(const SGVec3d& aCart) const
247{
248 assert(contains(aCart));
249 int childIndex = 0;
250
251 SGVec3d center(_box.getCenter());
252// tests must match indices in SGbox::getCorner
253 if (aCart.x() < center.x()) {
254 childIndex += 1;
255 }
256
257 if (aCart.y() < center.y()) {
258 childIndex += 2;
259 }
260
261 if (aCart.z() < center.z()) {
262 childIndex += 4;
263 }
264
265 return childAtIndex(childIndex);
266}
267
268Node* Branch::childAtIndex(int childIndex) const
269{
270 Node* child = _children[childIndex];
271 if (!child) { // lazy building of children
272 SGBoxd cb(boxForChild(childIndex));
273 double d2 = dot(cb.getSize(), cb.getSize());
274
275 assert(((_ident << 3) >> 3) == _ident);
276
277 // child index is 0..7, so 3-bits is sufficient, and hence we can
278 // pack 20 levels of octree into a int64, which is plenty
279 int64_t childIdent = (_ident << 3) | childIndex;
280
281 if (d2 < LEAF_SIZE_SQR) {
282 // REVIEW: Memory Leak - 480 bytes in 3 blocks are still reachable
283 child = new Leaf(cb, childIdent, _persistent);
284 } else {
285 // REVIEW: Memory Leak - 9,152 bytes in 52 blocks are still reachable
286 child = new Branch(cb, childIdent, _persistent);
287 }
288
289 _children[childIndex] = child;
290
291 if (_persistent && _childrenLoaded) {
292 // childrenLoad is done, so we're defining a new node - add it to the
293 // cache too.
294 NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
295 }
296 }
297
298 return _children[childIndex];
299}
300
301void Branch::loadChildren() const
302{
303 if (_childrenLoaded) {
304 return;
305 }
306
307 int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
308 for (int i=0; i<8; ++i) {
309 if ((1 << i) & childrenMask) {
310 childAtIndex(i); // accessing will create!
311 }
312 } // of child index iteration
313
314// set this after creating the child nodes, so the cache update logic
315// in childAtIndex knows any future created children need to be added.
316 _childrenLoaded = true;
317}
318
320{
321 int result = 0;
322 for (int i=0; i<8; ++i) {
323 if (_children[i]) {
324 result |= 1 << i;
325 }
326 }
327
328 return result;
329}
330
331bool findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
332{
333 aResults.clear();
335 FindNearestResults results;
338 double cut = aCutoffM;
339
340 SGTimeStamp tm;
341 tm.stamp();
342
343 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
344 if (!results.empty()) {
345 // terminate the search if we have sufficent results, and we are
346 // sure no node still on the queue contains a closer match
347 double furthestResultOrder = results.back().order();
348 if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
349 // clear the PQ to mark this has 'full results' instead of partial
350 pq = FindNearestPQueue();
351 break;
352 }
353 }
354
355 Node* nd = pq.top().get();
356 pq.pop();
357
358 nd->visit(aPos, cut, aFilter, results, pq);
359 } // of queue iteration
360
361 // depending on leaf population, we may have (slighty) more results
362 // than requested
363 unsigned int numResults = std::min((unsigned int) results.size(), aN);
364 // copy results out
365 aResults.resize(numResults);
366 for (unsigned int r=0; r<numResults; ++r) {
367 aResults[r] = results[r].get();
368 }
369
370 return !pq.empty();
371}
372
373bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
374{
375 aResults.clear();
377 FindNearestResults results;
380 double rng = aRangeM;
381
382 SGTimeStamp tm;
383 tm.stamp();
384
385 while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
386 Node* nd = pq.top().get();
387 pq.pop();
388
389 nd->visit(aPos, rng, aFilter, results, pq);
390 } // of queue iteration
391
392 unsigned int numResults = results.size();
393 // copy results out
394 aResults.resize(numResults);
395 for (unsigned int r=0; r<numResults; ++r) {
396 aResults[r] = results[r].get();
397 }
398
399 return !pq.empty();
400}
401
402} // of namespace Octree
403
404} // of namespace flightgear
#define p(x)
#define i(x)
Predicate class to support custom filtering of FGPositioned queries Default implementation of this pa...
virtual Type maxType() const
virtual Type minType() const
virtual bool pass(FGPositioned *aPos) const
Over-rideable filter method.
void defineOctreeNode(Octree::Branch *pr, Octree::Node *nd)
TypedPositionedVec getOctreeLeafChildren(int64_t octreeNodeId)
given an octree leaf, return all its child positioned items and their types
int getOctreeBranchChildren(int64_t octreeNodeId)
Given an Octree node ID, return a bit-mask defining which of the child nodes exist.
FGPositionedRef loadById(PositionedID guid)
retrieve an FGPositioned from the cache.
static NavDataCache * instance()
Branch(const SGBoxd &aBox, int64_t aIdent, bool persistent)
virtual Node * findNodeForBox(const SGBoxd &box) const
virtual void visit(const SGVec3d &aPos, double aCutoff, FGPositioned::Filter *, FindNearestResults &, FindNearestPQueue &aQ)
virtual void visitForLines(const SGVec3d &aPos, double aCutoff, PolyLineList &aLines, FindLinesDeque &aQ) const
Leaf(const SGBoxd &aBox, int64_t aIdent, bool persistent)
void removeChild(PositionedID id)
virtual void visit(const SGVec3d &aPos, double aCutoff, FGPositioned::Filter *aFilter, FindNearestResults &aResults, FindNearestPQueue &)
void insertChild(FGPositioned::Type ty, PositionedID id)
Octree node base class, tracks its bounding box and provides various queries relating to it.
virtual void visit(const SGVec3d &aPos, double aCutoff, FGPositioned::Filter *aFilter, FindNearestResults &aResults, FindNearestPQueue &)=0
void addPolyLine(const PolyLineRef &)
virtual void visitForLines(const SGVec3d &aPos, double aCutoff, PolyLineList &aLines, FindLinesDeque &aQ) const
Node(const SGBoxd &aBox, int64_t aIdent, bool persistent)
bool contains(const SGVec3d &aPos) const
virtual Node * findNodeForBox(const SGBoxd &box) const
Decorate an object with a double value, and use that value to order items, for the purpoises of the S...
static std::unique_ptr< Node > global_transientOctree
Ordered< FGPositioned * > OrderedPositioned
std::deque< Node * > FindLinesDeque
static std::unique_ptr< Node > global_spatialOctree
bool findAllWithinRange(const SGVec3d &aPos, double aRangeM, FGPositioned::Filter *aFilter, FGPositionedList &aResults, int aCutoffMsec)
std::vector< OrderedPositioned > FindNearestResults
bool findNearestN(const SGVec3d &aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter *aFilter, FGPositionedList &aResults, int aCutoffMsec)
std::priority_queue< OrderedNode, std::vector< OrderedNode >, FNPQCompare > FindNearestPQueue
the priority queue is fundamental to our search algorithm.
static bool boxContainsBox(const SGBoxd &a, const SGBoxd &b)
FlightPlan.hxx - defines a full flight-plan object, including departure, cruise, arrival information ...
Definition Addon.cxx:53
std::pair< FGPositioned::Type, PositionedID > TypedPositioned
std::vector< PolyLineRef > PolyLineList
Definition PolyLine.hxx:41
SGSharedPtr< PolyLine > PolyLineRef
Definition PolyLine.hxx:40
std::vector< FGPositionedRef > FGPositionedList
int64_t PositionedID