08d82294bd
When a position is modified for an in-cache FGPositioned, we need to update the runtime information too, or the Octree code may (rightly) complain that it's seeing inconsistent data. Also make the Octree check an exception throw, and verbose, so this is easier to detect in the future.
302 lines
8 KiB
C++
302 lines
8 KiB
C++
/**
|
|
* PositionedOctree - define a spatial octree containing Positioned items
|
|
* arranged by their global cartesian position.
|
|
*/
|
|
|
|
// Written by James Turner, started 2012.
|
|
//
|
|
// Copyright (C) 2012 James Turner
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful, but
|
|
// WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
# include "config.h"
|
|
#endif
|
|
|
|
#include "PositionedOctree.hxx"
|
|
#include "positioned.hxx"
|
|
|
|
#include <cassert>
|
|
#include <algorithm> // for sort
|
|
#include <cstring> // for memset
|
|
#include <iostream>
|
|
|
|
#include <boost/foreach.hpp>
|
|
|
|
#include <simgear/debug/logstream.hxx>
|
|
#include <simgear/structure/exception.hxx>
|
|
|
|
namespace flightgear
|
|
{
|
|
|
|
namespace Octree
|
|
{
|
|
|
|
Node* global_spatialOctree = NULL;
|
|
|
|
Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent) :
|
|
Node(aBox, aIdent),
|
|
childrenLoaded(false)
|
|
{
|
|
}
|
|
|
|
void Leaf::visit(const SGVec3d& aPos, double aCutoff,
|
|
FGPositioned::Filter* aFilter,
|
|
FindNearestResults& aResults, FindNearestPQueue&)
|
|
{
|
|
int previousResultsSize = aResults.size();
|
|
int addedCount = 0;
|
|
NavDataCache* cache = NavDataCache::instance();
|
|
|
|
loadChildren();
|
|
|
|
ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
|
|
ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
|
|
|
|
for (; it != end; ++it) {
|
|
FGPositioned* p = cache->loadById(it->second);
|
|
if (!intersects(_box, p->cart())) {
|
|
// see http://code.google.com/p/flightgear-bugs/issues/detail?id=905
|
|
|
|
SG_LOG(SG_GENERAL, SG_WARN, "XXXXXXXXX bad spatial index for " << it->second
|
|
<< "; " << p->ident() << " of type " <<
|
|
FGPositioned::nameForType(p->type()));
|
|
SG_LOG(SG_GENERAL, SG_WARN, "\tgeodetic location:" << p->geod());
|
|
SG_LOG(SG_GENERAL, SG_WARN, "\tcartesian location:" << p->cart());
|
|
|
|
SG_LOG(SG_GENERAL, SG_WARN, "leaf box:" <<
|
|
_box.getMin() << " x " << _box.getMax());
|
|
|
|
throw sg_exception("Bad spatial index data");
|
|
}
|
|
|
|
|
|
double d = dist(aPos, p->cart());
|
|
if (d > aCutoff) {
|
|
continue;
|
|
}
|
|
|
|
if (aFilter && !aFilter->pass(p)) {
|
|
continue;
|
|
}
|
|
|
|
++addedCount;
|
|
aResults.push_back(OrderedPositioned(p, d));
|
|
}
|
|
|
|
if (addedCount == 0) {
|
|
return;
|
|
}
|
|
|
|
// keep aResults sorted
|
|
// sort the new items, usually just one or two items
|
|
std::sort(aResults.begin() + previousResultsSize, aResults.end());
|
|
|
|
// merge the two sorted ranges together - in linear time
|
|
std::inplace_merge(aResults.begin(),
|
|
aResults.begin() + previousResultsSize, aResults.end());
|
|
}
|
|
|
|
void Leaf::insertChild(FGPositioned::Type ty, PositionedID id)
|
|
{
|
|
assert(childrenLoaded);
|
|
children.insert(children.end(), TypedPositioned(ty, id));
|
|
}
|
|
|
|
void Leaf::loadChildren()
|
|
{
|
|
if (childrenLoaded) {
|
|
return;
|
|
}
|
|
|
|
NavDataCache* cache = NavDataCache::instance();
|
|
BOOST_FOREACH(TypedPositioned tp, cache->getOctreeLeafChildren(guid())) {
|
|
children.insert(children.end(), tp);
|
|
} // of leaf members iteration
|
|
|
|
childrenLoaded = true;
|
|
}
|
|
|
|
Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
|
|
Node(aBox, aIdent),
|
|
childrenLoaded(false)
|
|
{
|
|
memset(children, 0, sizeof(Node*) * 8);
|
|
}
|
|
|
|
void Branch::visit(const SGVec3d& aPos, double aCutoff,
|
|
FGPositioned::Filter*,
|
|
FindNearestResults&, FindNearestPQueue& aQ)
|
|
{
|
|
loadChildren();
|
|
for (unsigned int i=0; i<8; ++i) {
|
|
if (!children[i]) {
|
|
continue;
|
|
}
|
|
|
|
double d = children[i]->distToNearest(aPos);
|
|
if (d > aCutoff) {
|
|
continue; // exceeded cutoff
|
|
}
|
|
|
|
aQ.push(Ordered<Node*>(children[i], d));
|
|
} // of child iteration
|
|
}
|
|
|
|
Node* Branch::childForPos(const SGVec3d& aCart) const
|
|
{
|
|
assert(contains(aCart));
|
|
int childIndex = 0;
|
|
|
|
SGVec3d center(_box.getCenter());
|
|
// tests must match indices in SGbox::getCorner
|
|
if (aCart.x() < center.x()) {
|
|
childIndex += 1;
|
|
}
|
|
|
|
if (aCart.y() < center.y()) {
|
|
childIndex += 2;
|
|
}
|
|
|
|
if (aCart.z() < center.z()) {
|
|
childIndex += 4;
|
|
}
|
|
|
|
return childAtIndex(childIndex);
|
|
}
|
|
|
|
Node* Branch::childAtIndex(int childIndex) const
|
|
{
|
|
Node* child = children[childIndex];
|
|
if (!child) { // lazy building of children
|
|
SGBoxd cb(boxForChild(childIndex));
|
|
double d2 = dot(cb.getSize(), cb.getSize());
|
|
|
|
assert(((_ident << 3) >> 3) == _ident);
|
|
|
|
// child index is 0..7, so 3-bits is sufficient, and hence we can
|
|
// pack 20 levels of octree into a int64, which is plenty
|
|
int64_t childIdent = (_ident << 3) | childIndex;
|
|
|
|
if (d2 < LEAF_SIZE_SQR) {
|
|
child = new Leaf(cb, childIdent);
|
|
} else {
|
|
child = new Branch(cb, childIdent);
|
|
}
|
|
|
|
children[childIndex] = child;
|
|
|
|
if (childrenLoaded) {
|
|
// childrenLoad is done, so we're defining a new node - add it to the
|
|
// cache too.
|
|
NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
|
|
}
|
|
}
|
|
|
|
return children[childIndex];
|
|
}
|
|
|
|
void Branch::loadChildren() const
|
|
{
|
|
if (childrenLoaded) {
|
|
return;
|
|
}
|
|
|
|
int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
|
|
for (int i=0; i<8; ++i) {
|
|
if ((1 << i) & childrenMask) {
|
|
childAtIndex(i); // accessing will create!
|
|
}
|
|
} // of child index iteration
|
|
|
|
// set this after creating the child nodes, so the cache update logic
|
|
// in childAtIndex knows any future created children need to be added.
|
|
childrenLoaded = true;
|
|
}
|
|
|
|
int Branch::childMask() const
|
|
{
|
|
int result = 0;
|
|
for (int i=0; i<8; ++i) {
|
|
if (children[i]) {
|
|
result |= 1 << i;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
|
|
{
|
|
aResults.clear();
|
|
FindNearestPQueue pq;
|
|
FindNearestResults results;
|
|
pq.push(Ordered<Node*>(global_spatialOctree, 0));
|
|
double cut = 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();
|
|
// std::cout << "furthest result:" << furthestResultOrder << ", top node order:" << pq.top().order() << std::endl;
|
|
if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
Node* nd = pq.top().get();
|
|
pq.pop();
|
|
|
|
// std::cout << "visiting:" << std::oct << nd->guid() << "(" << std::dec << nd->guid() << ")" << std::endl;
|
|
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;
|
|
|
|
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();
|
|
}
|
|
}
|
|
|
|
} // of namespace Octree
|
|
|
|
} // of namespace flightgear
|