1 // positioned.cxx - base class for objects which are positioned
3 // Copyright (C) 2008 James Turner
5 // This program is free software; you can redistribute it and/or
6 // modify it under the terms of the GNU General Public License as
7 // published by the Free Software Foundation; either version 2 of the
8 // License, or (at your option) any later version.
10 // This program is distributed in the hope that it will be useful, but
11 // WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
15 // You should have received a copy of the GNU General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
25 #include "positioned.hxx"
29 #include <algorithm> // for sort
33 #include <boost/foreach.hpp>
34 #include <boost/algorithm/string/case_conv.hpp>
35 #include <boost/algorithm/string/predicate.hpp>
37 #include <simgear/timing/timestamp.hxx>
38 #include <simgear/debug/logstream.hxx>
39 #include <simgear/structure/exception.hxx>
40 #include <simgear/math/SGGeometry.hxx>
41 #include <simgear/sg_inlines.h>
43 #include "Navaids/PositionedOctree.hxx"
46 using namespace flightgear;
48 static void validateSGGeod(const SGGeod& geod)
50 if (SGMisc<double>::isNaN(geod.getLatitudeDeg()) ||
51 SGMisc<double>::isNaN(geod.getLongitudeDeg()))
53 throw sg_range_exception("position is invalid, NaNs");
57 static bool validateFilter(FGPositioned::Filter* filter)
59 if (filter->maxType() < filter->minType()) {
60 SG_LOG(SG_GENERAL, SG_WARN, "invalid positioned filter specified");
68 ///////////////////////////////////////////////////////////////////////////////
70 FGPositioned::FGPositioned( PositionedID aGuid,
72 const std::string& aIdent,
78 mCart(SGVec3d::fromGeod(mPosition))
83 FGPositioned::~FGPositioned()
85 // std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl;
89 FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos)
91 NavDataCache* cache = NavDataCache::instance();
92 TypeFilter filter(WAYPOINT);
93 FGPositionedList existing = cache->findAllWithIdent(aIdent, &filter, true);
94 if (!existing.empty()) {
95 SG_LOG(SG_NAVAID, SG_WARN, "attempt to insert duplicate WAYPOINT:" << aIdent);
96 return existing.front().ptr();
99 PositionedID id = cache->createPOI(WAYPOINT, aIdent, aPos);
100 return cache->loadById(id);
103 bool FGPositioned::deleteUserWaypoint(const std::string& aIdent)
105 NavDataCache* cache = NavDataCache::instance();
106 return cache->removePOI(WAYPOINT, aIdent);
111 FGPositioned::cart() const
116 FGPositioned::Type FGPositioned::typeFromName(const std::string& aName)
118 if (aName.empty() || (aName == "")) {
127 const NameTypeEntry names[] = {
128 {"airport", AIRPORT},
140 {"tower", FREQ_TOWER},
141 {"ground", FREQ_GROUND},
142 {"approach", FREQ_APP_DEP},
143 {"departure", FREQ_APP_DEP},
145 {"helipad", HELIPAD},
146 {"country", COUNTRY},
149 {"village", VILLAGE},
153 {"gnd", FREQ_GROUND},
155 {"waypoint", WAYPOINT},
165 std::string lowerName(boost::to_lower_copy(aName));
167 for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) {
168 if (::strcmp(n->_name, lowerName.c_str()) == 0) {
173 SG_LOG(SG_NAVAID, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName);
177 const char* FGPositioned::nameForType(Type aTy)
180 case RUNWAY: return "runway";
181 case HELIPAD: return "helipad";
182 case TAXIWAY: return "taxiway";
183 case PAVEMENT: return "pavement";
184 case PARKING: return "parking stand";
185 case FIX: return "fix";
186 case VOR: return "VOR";
187 case NDB: return "NDB";
188 case ILS: return "ILS";
189 case LOC: return "localizer";
190 case GS: return "glideslope";
191 case OM: return "outer-marker";
192 case MM: return "middle-marker";
193 case IM: return "inner-marker";
194 case AIRPORT: return "airport";
195 case HELIPORT: return "heliport";
196 case SEAPORT: return "seaport";
197 case WAYPOINT: return "waypoint";
198 case DME: return "dme";
199 case TACAN: return "tacan";
200 case FREQ_TOWER: return "tower";
201 case FREQ_ATIS: return "atis";
202 case FREQ_AWOS: return "awos";
203 case FREQ_GROUND: return "ground";
204 case FREQ_CLEARANCE: return "clearance";
205 case FREQ_UNICOM: return "unicom";
206 case FREQ_APP_DEP: return "approach-departure";
207 case TAXI_NODE: return "taxi-node";
208 case COUNTRY: return "country";
209 case CITY: return "city";
210 case TOWN: return "town";
211 case VILLAGE: return "village";
217 ///////////////////////////////////////////////////////////////////////////////
218 // search / query functions
221 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
223 validateSGGeod(aPos);
224 return NavDataCache::instance()->findClosestWithIdent(aIdent, aPos, aFilter);
228 FGPositioned::findFirstWithIdent(const std::string& aIdent, Filter* aFilter)
230 if (aIdent.empty()) {
235 NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, true);
244 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
246 validateSGGeod(aPos);
248 if (!validateFilter(aFilter)) {
249 return FGPositionedList();
252 FGPositionedList result;
253 Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
254 aRangeNm * SG_NM_TO_METER, aFilter, result, 0xffffff);
259 FGPositioned::findWithinRangePartial(const SGGeod& aPos, double aRangeNm, Filter* aFilter, bool& aPartial)
261 validateSGGeod(aPos);
263 if (!validateFilter(aFilter)) {
264 return FGPositionedList();
268 FGPositionedList result;
269 aPartial = Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
270 aRangeNm * SG_NM_TO_METER, aFilter, result,
276 FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
278 if (!validateFilter(aFilter)) {
279 return FGPositionedList();
282 return NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, aExact);
286 FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
288 if (!validateFilter(aFilter)) {
289 return FGPositionedList();
292 return NavDataCache::instance()->findAllWithName(aName, aFilter, aExact);
296 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
298 validateSGGeod(aPos);
300 if (!validateFilter(aFilter)) {
304 FGPositionedList l(findClosestN(aPos, 1, aCutoffNm, aFilter));
309 assert(l.size() == 1);
314 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
316 validateSGGeod(aPos);
318 FGPositionedList result;
319 int limitMsec = 0xffff;
320 Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result, limitMsec);
325 FGPositioned::findClosestNPartial(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter, bool &aPartial)
327 validateSGGeod(aPos);
329 FGPositionedList result;
331 aPartial = Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result,
337 FGPositioned::sortByRange(FGPositionedList& aResult, const SGGeod& aPos)
339 validateSGGeod(aPos);
341 SGVec3d cartPos(SGVec3d::fromGeod(aPos));
342 // computer ordering values
343 Octree::FindNearestResults r;
344 FGPositionedList::iterator it = aResult.begin(), lend = aResult.end();
345 for (; it != lend; ++it) {
346 double d2 = distSqr((*it)->cart(), cartPos);
347 r.push_back(Octree::OrderedPositioned(*it, d2));
351 std::sort(r.begin(), r.end());
353 // convert to a plain list
354 unsigned int count = aResult.size();
355 for (unsigned int i=0; i<count; ++i) {
356 aResult[i] = r[i].get();
360 //------------------------------------------------------------------------------
361 void FGPositioned::modifyPosition(const SGGeod& newPos)
363 const_cast<SGGeod&>(mPosition) = newPos;
364 const_cast<SGVec3d&>(mCart) = SGVec3d::fromGeod(newPos);
367 //------------------------------------------------------------------------------
368 void FGPositioned::invalidatePosition()
370 const_cast<SGGeod&>(mPosition) = SGGeod::fromDeg(999,999);
371 const_cast<SGVec3d&>(mCart) = SGVec3d::zeros();
374 //------------------------------------------------------------------------------
375 FGPositionedRef FGPositioned::loadByIdImpl(PositionedID id)
377 return flightgear::NavDataCache::instance()->loadById(id);
380 FGPositioned::TypeFilter::TypeFilter(Type aTy) :
387 void FGPositioned::TypeFilter::addType(Type aTy)
389 if (aTy == INVALID) {
393 types.push_back(aTy);
394 mMinType = std::min(mMinType, aTy);
395 mMaxType = std::max(mMaxType, aTy);
398 FGPositioned::TypeFilter
399 FGPositioned::TypeFilter::fromString(const std::string& aFilterSpec)
401 if (aFilterSpec.empty()) {
402 throw sg_format_exception("empty filter spec:", aFilterSpec);
405 string_list parts = simgear::strutils::split(aFilterSpec, ",");
408 BOOST_FOREACH(std::string token, parts) {
409 f.addType(typeFromName(token));
416 FGPositioned::TypeFilter::pass(FGPositioned* aPos) const
422 std::vector<Type>::const_iterator it = types.begin(),
424 for (; it != end; ++it) {
425 if (aPos->type() == *it) {