1 // Copyright (C) 2009 - 2010 Mathias Froehlich - Mathias.Froehlich@web.de
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Library General Public
5 // License as published by the Free Software Foundation; either
6 // version 2 of the License, or (at your option) any later version.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Library General Public License for more details.
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 #ifndef HLALocation_hxx
19 #define HLALocation_hxx
21 #include <simgear/math/SGMath.hxx>
22 #include "HLABasicDataElement.hxx"
23 #include "HLATypes.hxx"
27 class HLAAbstractLocation : public SGReferenced {
29 virtual ~HLAAbstractLocation() {}
31 virtual SGLocationd getLocation() const = 0;
32 virtual void setLocation(const SGLocationd&) = 0;
34 virtual SGVec3d getCartPosition() const = 0;
35 virtual void setCartPosition(const SGVec3d&) = 0;
37 virtual SGQuatd getCartOrientation() const = 0;
38 virtual void setCartOrientation(const SGQuatd&) = 0;
40 virtual SGVec3d getAngularBodyVelocity() const = 0;
41 virtual void setAngularBodyVelocity(const SGVec3d&) = 0;
43 virtual SGVec3d getLinearBodyVelocity() const = 0;
44 virtual void setLinearBodyVelocity(const SGVec3d&) = 0;
46 virtual double getTimeDifference(const SGTimeStamp&) const
49 // Get the position and orientation extrapolated to the given time stamp.
50 SGLocationd getLocation(const SGTimeStamp& timeStamp) const
52 SGLocationd location = getLocation();
53 location.eulerStepBodyVelocities(getTimeDifference(timeStamp), getLinearBodyVelocity(), getAngularBodyVelocity());
58 class HLACartesianLocation : public HLAAbstractLocation {
60 HLACartesianLocation() :
61 _position(SGVec3d::zeros()),
62 _imag(SGVec3d::zeros()),
63 _angularVelocity(SGVec3d::zeros()),
64 _linearVelocity(SGVec3d::zeros())
67 virtual SGLocationd getLocation() const
68 { return SGLocationd(_position, SGQuatd::fromPositiveRealImag(_imag)); }
69 virtual void setLocation(const SGLocationd& location)
71 _position = location.getPosition();
72 _imag = location.getOrientation().getPositiveRealImag();
75 virtual SGVec3d getCartPosition() const
77 virtual void setCartPosition(const SGVec3d& position)
78 { _position = position; }
80 virtual SGQuatd getCartOrientation() const
81 { return SGQuatd::fromPositiveRealImag(_imag); }
82 virtual void setCartOrientation(const SGQuatd& orientation)
83 { _imag = orientation.getPositiveRealImag(); }
85 virtual SGVec3d getAngularBodyVelocity() const
86 { return _angularVelocity; }
87 virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
88 { _angularVelocity = angularVelocity; }
90 virtual SGVec3d getLinearBodyVelocity() const
91 { return _linearVelocity; }
92 virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
93 { _linearVelocity = linearVelocity; }
95 HLADataElement* getPositionDataElement(unsigned i)
99 return new PositionDataElement(this, i);
101 HLADataElement* getOrientationDataElement(unsigned i)
105 return new OrientationDataElement(this, i);
108 HLADataElement* getAngularVelocityDataElement(unsigned i)
112 return new AngularVelocityDataElement(this, i);
114 HLADataElement* getLinearVelocityDataElement(unsigned i)
118 return new LinearVelocityDataElement(this, i);
122 class PositionDataElement : public HLAAbstractDoubleDataElement {
124 PositionDataElement(HLACartesianLocation* data, unsigned index) :
125 _data(data), _index(index)
127 virtual double getValue() const
128 { return _data->_position[_index]; }
129 virtual void setValue(double value)
130 { _data->_position[_index] = value; }
133 SGSharedPtr<HLACartesianLocation> _data;
137 class OrientationDataElement : public HLAAbstractDoubleDataElement {
139 OrientationDataElement(HLACartesianLocation* data, unsigned index) :
140 _data(data), _index(index)
142 virtual double getValue() const
143 { return _data->_imag[_index]; }
144 virtual void setValue(double value)
145 { _data->_imag[_index] = value; }
148 SGSharedPtr<HLACartesianLocation> _data;
152 class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
154 AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
155 _data(data), _index(index)
157 virtual double getValue() const
158 { return _data->_angularVelocity[_index]; }
159 virtual void setValue(double value)
160 { _data->_angularVelocity[_index] = value; }
163 SGSharedPtr<HLACartesianLocation> _data;
167 class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
169 LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
170 _data(data), _index(index)
172 virtual double getValue() const
173 { return _data->_linearVelocity[_index]; }
174 virtual void setValue(double value)
175 { _data->_linearVelocity[_index] = value; }
178 SGSharedPtr<HLACartesianLocation> _data;
185 SGVec3d _angularVelocity;
186 SGVec3d _linearVelocity;
189 class HLALocationFactory : public SGReferenced {
191 virtual ~HLALocationFactory() {}
192 virtual HLAAbstractLocation* createLocation(HLAObjectInstance&) const = 0;
195 class HLACartesianLocationFactory : public HLALocationFactory {
197 virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const
199 HLACartesianLocation* location = new HLACartesianLocation;
201 for (unsigned i = 0; i < 3; ++i)
202 objectInstance.setAttributeDataElement(_positonIndex[i], location->getPositionDataElement(i));
203 for (unsigned i = 0; i < 3; ++i)
204 objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
205 for (unsigned i = 0; i < 3; ++i)
206 objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
207 for (unsigned i = 0; i < 3; ++i)
208 objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
213 void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
217 _positonIndex[index] = dataElementIndex;
219 void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
223 _orientationIndex[index] = dataElementIndex;
226 void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
230 _angularVelocityIndex[index] = dataElementIndex;
232 void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
236 _linearVelocityIndex[index] = dataElementIndex;
240 HLADataElementIndex _positonIndex[3];
241 HLADataElementIndex _orientationIndex[3];
243 HLADataElementIndex _angularVelocityIndex[3];
244 HLADataElementIndex _linearVelocityIndex[3];
247 class HLAGeodeticLocation : public HLAAbstractLocation {
267 VerticalSpeedFtPerSec,
268 VerticalSpeedFtPerMin,
272 HLAGeodeticLocation() :
274 _cartPosition(SGVec3d::zeros()),
275 _cartOrientation(SGQuatd::unit()),
276 _cartBodyVelocity(SGVec3d::zeros()),
278 _geodEulerRad(SGVec3d::zeros()),
280 _groundSpeedMPerSec(0),
281 _verticalSpeedMPerSec(0)
283 updateCartesianFromGeodetic();
286 virtual SGLocationd getLocation() const
288 updateCartesianFromGeodetic();
289 return SGLocationd(_cartPosition, _cartOrientation);
291 virtual void setLocation(const SGLocationd& location)
293 _cartPosition = location.getPosition();
294 _cartOrientation = location.getOrientation();
298 virtual SGVec3d getCartPosition() const
299 { updateCartesianFromGeodetic(); return _cartPosition; }
300 virtual void setCartPosition(const SGVec3d& position)
301 { _cartPosition = position; _dirty = true; }
303 virtual SGQuatd getCartOrientation() const
304 { updateCartesianFromGeodetic(); return _cartOrientation; }
305 virtual void setCartOrientation(const SGQuatd& orientation)
306 { _cartOrientation = orientation; _dirty = true; }
308 virtual SGVec3d getAngularBodyVelocity() const
309 { return SGVec3d::zeros(); }
310 virtual void setAngularBodyVelocity(const SGVec3d& angular)
313 virtual SGVec3d getLinearBodyVelocity() const
314 { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
315 virtual void setLinearBodyVelocity(const SGVec3d& linear)
316 { _cartBodyVelocity = linear; _dirty = true; }
318 void setLatitudeDeg(double value)
319 { _geodPosition.setLatitudeDeg(value); _dirty = true; }
320 double getLatitudeDeg() const
321 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
322 void setLatitudeRad(double value)
323 { _geodPosition.setLatitudeRad(value); _dirty = true; }
324 double getLatitudeRad() const
325 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
326 void setLongitudeDeg(double value)
327 { _geodPosition.setLongitudeDeg(value); _dirty = true; }
328 double getLongitudeDeg() const
329 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
330 void setLongitudeRad(double value)
331 { _geodPosition.setLongitudeRad(value); _dirty = true; }
332 double getLongitudeRad() const
333 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
334 void setElevationFt(double value)
335 { _geodPosition.setElevationFt(value); _dirty = true; }
336 double getElevationFt() const
337 { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
338 void setElevationM(double value)
339 { _geodPosition.setElevationM(value); _dirty = true; }
340 double getElevationM() const
341 { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
343 void setHeadingRad(double value)
344 { _geodEulerRad[2] = value; _dirty = true; }
345 double getHeadingRad() const
346 { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
347 void setHeadingDeg(double value)
348 { setHeadingRad(SGMiscd::deg2rad(value)); }
349 double getHeadingDeg() const
350 { return SGMiscd::rad2deg(getHeadingRad()); }
351 void setPitchRad(double value)
352 { _geodEulerRad[1] = value; _dirty = true; }
353 double getPitchRad() const
354 { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
355 void setPitchDeg(double value)
356 { setPitchRad(SGMiscd::deg2rad(value)); }
357 double getPitchDeg() const
358 { return SGMiscd::rad2deg(getPitchRad()); }
359 void setRollRad(double value)
360 { _geodEulerRad[0] = value; _dirty = true; }
361 double getRollRad() const
362 { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
363 void setRollDeg(double value)
364 { setRollRad(SGMiscd::deg2rad(value)); }
365 double getRollDeg() const
366 { return SGMiscd::rad2deg(getRollRad()); }
368 void setGroundTrackRad(double value)
369 { _groundTrackRad = value; _dirty = true; }
370 double getGroundTrackRad() const
371 { updateGeodeticFromCartesian(); return _groundTrackRad; }
372 void setGroundTrackDeg(double value)
373 { setGroundTrackRad(SGMiscd::deg2rad(value)); }
374 double getGroundTrackDeg() const
375 { return SGMiscd::rad2deg(getGroundTrackRad()); }
378 void setGroundSpeedMPerSec(double value)
379 { _groundSpeedMPerSec = value; _dirty = true; }
380 double getGroundSpeedMPerSec() const
381 { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
382 void setGroundSpeedFtPerSec(double value)
383 { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
384 double getGroundSpeedFtPerSec() const
385 { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
386 void setGroundSpeedKnots(double value)
387 { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
388 double getGroundSpeedKnots() const
389 { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
391 void setVerticalSpeedMPerSec(double value)
392 { _verticalSpeedMPerSec = value; _dirty = true; }
393 double getVerticalSpeedMPerSec() const
394 { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
395 void setVerticalSpeedFtPerSec(double value)
396 { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
397 double getVerticalSpeedFtPerSec() const
398 { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
399 void setVerticalSpeedFtPerMin(double value)
400 { setVerticalSpeedFtPerSec(value/60); }
401 double getVerticalSpeedFtPerMin() const
402 { return 60*getVerticalSpeedFtPerSec(); }
404 #define DATA_ELEMENT(name) \
405 HLADataElement* get## name ## DataElement() \
406 { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
408 DATA_ELEMENT(LatitudeDeg)
409 DATA_ELEMENT(LatitudeRad)
410 DATA_ELEMENT(LongitudeDeg)
411 DATA_ELEMENT(LongitudeRad)
412 DATA_ELEMENT(ElevationFt)
413 DATA_ELEMENT(ElevationM)
414 DATA_ELEMENT(HeadingDeg)
415 DATA_ELEMENT(HeadingRad)
416 DATA_ELEMENT(PitchDeg)
417 DATA_ELEMENT(PitchRad)
418 DATA_ELEMENT(RollDeg)
419 DATA_ELEMENT(RollRad)
421 DATA_ELEMENT(GroundTrackDeg)
422 DATA_ELEMENT(GroundTrackRad)
423 DATA_ELEMENT(GroundSpeedMPerSec)
424 DATA_ELEMENT(GroundSpeedFtPerSec)
425 DATA_ELEMENT(GroundSpeedKnots)
426 DATA_ELEMENT(VerticalSpeedMPerSec)
427 DATA_ELEMENT(VerticalSpeedFtPerSec)
428 DATA_ELEMENT(VerticalSpeedFtPerMin)
432 HLADataElement* getDataElement(Semantic semantic)
436 return getLatitudeDegDataElement();
438 return getLatitudeRadDataElement();
440 return getLongitudeDegDataElement();
442 return getLongitudeRadDataElement();
444 return getElevationMDataElement();
446 return getElevationFtDataElement();
448 return getHeadingDegDataElement();
450 return getHeadingRadDataElement();
452 return getPitchDegDataElement();
454 return getPitchRadDataElement();
456 return getRollDegDataElement();
458 return getRollRadDataElement();
460 return getGroundTrackDegDataElement();
462 return getGroundTrackRadDataElement();
463 case GroundSpeedKnots:
464 return getGroundSpeedKnotsDataElement();
465 case GroundSpeedFtPerSec:
466 return getGroundSpeedFtPerSecDataElement();
467 case GroundSpeedMPerSec:
468 return getGroundSpeedMPerSecDataElement();
469 case VerticalSpeedFtPerSec:
470 return getVerticalSpeedFtPerSecDataElement();
471 case VerticalSpeedFtPerMin:
472 return getVerticalSpeedFtPerMinDataElement();
473 case VerticalSpeedMPerSec:
474 return getVerticalSpeedMPerSecDataElement();
481 template<double (HLAGeodeticLocation::*getter)() const,
482 void (HLAGeodeticLocation::*setter)(double)>
483 class DataElement : public HLAAbstractDoubleDataElement {
485 DataElement(HLAGeodeticLocation* data) :
488 virtual double getValue() const
489 { return (_data->*getter)(); }
490 virtual void setValue(double value)
491 { (_data->*setter)(value); }
494 SGSharedPtr<HLAGeodeticLocation> _data;
497 void updateGeodeticFromCartesian() const
501 _geodPosition = SGGeod::fromCart(_cartPosition);
502 SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
503 geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
504 SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
505 if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
506 _groundTrackRad = atan2(nedVel[1], nedVel[0]);
509 _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
510 _verticalSpeedMPerSec = -nedVel[2];
513 void updateCartesianFromGeodetic() const
517 _cartPosition = SGVec3d::fromGeod(_geodPosition);
518 SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
519 _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
520 SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
521 sin(_groundTrackRad)*_groundSpeedMPerSec,
522 -_verticalSpeedMPerSec);
523 _cartBodyVelocity = geodOrientation.transform(nedVel);
529 // the cartesian values
530 mutable SGVec3d _cartPosition;
531 mutable SGQuatd _cartOrientation;
532 mutable SGVec3d _cartBodyVelocity;
534 // The geodetic values
535 mutable SGGeod _geodPosition;
536 mutable SGVec3d _geodEulerRad;
537 mutable double _groundTrackRad;
538 mutable double _groundSpeedMPerSec;
539 mutable double _verticalSpeedMPerSec;
542 class HLAGeodeticLocationFactory : public HLALocationFactory {
545 LatitudeDeg = HLAGeodeticLocation::LatitudeDeg,
546 LatitudeRad = HLAGeodeticLocation::LatitudeRad,
547 LongitudeDeg = HLAGeodeticLocation::LongitudeDeg,
548 LongitudeRad = HLAGeodeticLocation::LongitudeRad,
549 ElevationM = HLAGeodeticLocation::ElevationM,
550 ElevationFt = HLAGeodeticLocation::ElevationFt,
551 HeadingDeg = HLAGeodeticLocation::HeadingDeg,
552 HeadingRad = HLAGeodeticLocation::HeadingRad,
553 PitchDeg = HLAGeodeticLocation::PitchDeg,
554 PitchRad = HLAGeodeticLocation::PitchRad,
555 RollDeg = HLAGeodeticLocation::RollDeg,
556 RollRad = HLAGeodeticLocation::RollRad,
557 GroundTrackDeg = HLAGeodeticLocation::GroundTrackDeg,
558 GroundTrackRad = HLAGeodeticLocation::GroundTrackRad,
559 GroundSpeedKnots = HLAGeodeticLocation::GroundSpeedKnots,
560 GroundSpeedFtPerSec = HLAGeodeticLocation::GroundSpeedFtPerSec,
561 GroundSpeedMPerSec = HLAGeodeticLocation::GroundSpeedMPerSec,
562 VerticalSpeedFtPerSec = HLAGeodeticLocation::VerticalSpeedFtPerSec,
563 VerticalSpeedFtPerMin = HLAGeodeticLocation::VerticalSpeedFtPerMin,
564 VerticalSpeedMPerSec = HLAGeodeticLocation::VerticalSpeedMPerSec
567 virtual HLAGeodeticLocation* createLocation(HLAObjectInstance& objectInstance) const
569 HLAGeodeticLocation* location = new HLAGeodeticLocation;
571 for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
572 i != _indexSemanticMap.end(); ++i) {
573 HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
574 objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
580 void setIndex(Semantic semantic, const HLADataElementIndex& index)
581 { _indexSemanticMap[index] = semantic; }
584 typedef std::map<HLADataElementIndex, Semantic> IndexSemanticMap;
585 IndexSemanticMap _indexSemanticMap;
588 } // namespace simgear