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 "HLABasicDataElement.hxx"
22 #include "HLATypes.hxx"
26 class HLAAbstractLocation : public SGReferenced {
28 virtual ~HLAAbstractLocation() {}
30 virtual SGVec3d getCartPosition() const = 0;
31 virtual void setCartPosition(const SGVec3d&) = 0;
33 virtual SGQuatd getCartOrientation() const = 0;
34 virtual void setCartOrientation(const SGQuatd&) = 0;
36 virtual SGVec3d getAngularBodyVelocity() const = 0;
37 virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
39 virtual SGVec3d getLinearBodyVelocity() const = 0;
40 virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
43 class HLACartesianLocation : public HLAAbstractLocation {
45 HLACartesianLocation() :
46 _position(SGVec3d::zeros()),
47 _imag(SGVec3d::zeros()),
48 _angularVelocity(SGVec3d::zeros()),
49 _linearVelocity(SGVec3d::zeros())
52 virtual SGVec3d getCartPosition() const
54 virtual void setCartPosition(const SGVec3d& position)
55 { _position = position; }
57 virtual SGQuatd getCartOrientation() const
58 { return SGQuatd::fromPositiveRealImag(_imag); }
59 virtual void setCartOrientation(const SGQuatd& orientation)
60 { _imag = orientation.getPositiveRealImag(); }
62 virtual SGVec3d getAngularBodyVelocity() const
63 { return _angularVelocity; }
64 virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
65 { _angularVelocity = angularVelocity; }
67 virtual SGVec3d getLinearBodyVelocity() const
68 { return _linearVelocity; }
69 virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
70 { _linearVelocity = linearVelocity; }
72 HLADataElement* getPositionDataElement(unsigned i)
76 return new PositionDataElement(this, i);
78 HLADataElement* getOrientationDataElement(unsigned i)
82 return new OrientationDataElement(this, i);
85 HLADataElement* getAngularVelocityDataElement(unsigned i)
89 return new AngularVelocityDataElement(this, i);
91 HLADataElement* getLinearVelocityDataElement(unsigned i)
95 return new LinearVelocityDataElement(this, i);
99 class PositionDataElement : public HLAAbstractDoubleDataElement {
101 PositionDataElement(HLACartesianLocation* data, unsigned index) :
102 _data(data), _index(index)
104 virtual double getValue() const
105 { return _data->_position[_index]; }
106 virtual void setValue(double value)
107 { _data->_position[_index] = value; }
110 SGSharedPtr<HLACartesianLocation> _data;
114 class OrientationDataElement : public HLAAbstractDoubleDataElement {
116 OrientationDataElement(HLACartesianLocation* data, unsigned index) :
117 _data(data), _index(index)
119 virtual double getValue() const
120 { return _data->_imag[_index]; }
121 virtual void setValue(double value)
122 { _data->_imag[_index] = value; }
125 SGSharedPtr<HLACartesianLocation> _data;
129 class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
131 AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
132 _data(data), _index(index)
134 virtual double getValue() const
135 { return _data->_angularVelocity[_index]; }
136 virtual void setValue(double value)
137 { _data->_angularVelocity[_index] = value; }
140 SGSharedPtr<HLACartesianLocation> _data;
144 class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
146 LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
147 _data(data), _index(index)
149 virtual double getValue() const
150 { return _data->_linearVelocity[_index]; }
151 virtual void setValue(double value)
152 { _data->_linearVelocity[_index] = value; }
155 SGSharedPtr<HLACartesianLocation> _data;
162 SGVec3d _angularVelocity;
163 SGVec3d _linearVelocity;
166 class HLALocationFactory : public SGReferenced {
168 virtual ~HLALocationFactory() {}
169 virtual HLAAbstractLocation* createLocation(HLAObjectInstance&) const = 0;
172 class HLACartesianLocationFactory : public HLALocationFactory {
174 virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const
176 HLACartesianLocation* location = new HLACartesianLocation;
178 for (unsigned i = 0; i < 3; ++i)
179 objectInstance.setAttributeDataElement(_positonIndex[i], location->getPositionDataElement(i));
180 for (unsigned i = 0; i < 3; ++i)
181 objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
182 for (unsigned i = 0; i < 3; ++i)
183 objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
184 for (unsigned i = 0; i < 3; ++i)
185 objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
190 void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
194 _positonIndex[index] = dataElementIndex;
196 void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
200 _orientationIndex[index] = dataElementIndex;
203 void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
207 _angularVelocityIndex[index] = dataElementIndex;
209 void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
213 _linearVelocityIndex[index] = dataElementIndex;
217 HLADataElementIndex _positonIndex[3];
218 HLADataElementIndex _orientationIndex[3];
220 HLADataElementIndex _angularVelocityIndex[3];
221 HLADataElementIndex _linearVelocityIndex[3];
224 class HLAGeodeticLocation : public HLAAbstractLocation {
244 VerticalSpeedFtPerSec,
245 VerticalSpeedFtPerMin,
249 HLAGeodeticLocation() :
251 _cartPosition(SGVec3d::zeros()),
252 _cartOrientation(SGQuatd::unit()),
253 _cartBodyVelocity(SGVec3d::zeros()),
255 _geodEulerRad(SGVec3d::zeros()),
257 _groundSpeedMPerSec(0),
258 _verticalSpeedMPerSec(0)
260 updateCartesianFromGeodetic();
263 virtual SGVec3d getCartPosition() const
264 { updateCartesianFromGeodetic(); return _cartPosition; }
265 virtual void setCartPosition(const SGVec3d& position)
266 { _cartPosition = position; _dirty = true; }
268 virtual SGQuatd getCartOrientation() const
269 { updateCartesianFromGeodetic(); return _cartOrientation; }
270 virtual void setCartOrientation(const SGQuatd& orientation)
271 { _cartOrientation = orientation; _dirty = true; }
273 virtual SGVec3d getAngularBodyVelocity() const
274 { return SGVec3d::zeros(); }
275 virtual void setAngularBodyVelocity(const SGVec3d& angular)
278 virtual SGVec3d getLinearBodyVelocity() const
279 { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
280 virtual void setLinearBodyVelocity(const SGVec3d& linear)
281 { _cartBodyVelocity = linear; _dirty = true; }
283 void setLatitudeDeg(double value)
284 { _geodPosition.setLatitudeDeg(value); _dirty = true; }
285 double getLatitudeDeg() const
286 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
287 void setLatitudeRad(double value)
288 { _geodPosition.setLatitudeRad(value); _dirty = true; }
289 double getLatitudeRad() const
290 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
291 void setLongitudeDeg(double value)
292 { _geodPosition.setLongitudeDeg(value); _dirty = true; }
293 double getLongitudeDeg() const
294 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
295 void setLongitudeRad(double value)
296 { _geodPosition.setLongitudeRad(value); _dirty = true; }
297 double getLongitudeRad() const
298 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
299 void setElevationFt(double value)
300 { _geodPosition.setElevationFt(value); _dirty = true; }
301 double getElevationFt() const
302 { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
303 void setElevationM(double value)
304 { _geodPosition.setElevationM(value); _dirty = true; }
305 double getElevationM() const
306 { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
308 void setHeadingRad(double value)
309 { _geodEulerRad[2] = value; _dirty = true; }
310 double getHeadingRad() const
311 { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
312 void setHeadingDeg(double value)
313 { setHeadingRad(SGMiscd::deg2rad(value)); }
314 double getHeadingDeg() const
315 { return SGMiscd::rad2deg(getHeadingRad()); }
316 void setPitchRad(double value)
317 { _geodEulerRad[1] = value; _dirty = true; }
318 double getPitchRad() const
319 { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
320 void setPitchDeg(double value)
321 { setPitchRad(SGMiscd::deg2rad(value)); }
322 double getPitchDeg() const
323 { return SGMiscd::rad2deg(getPitchRad()); }
324 void setRollRad(double value)
325 { _geodEulerRad[0] = value; _dirty = true; }
326 double getRollRad() const
327 { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
328 void setRollDeg(double value)
329 { setRollRad(SGMiscd::deg2rad(value)); }
330 double getRollDeg() const
331 { return SGMiscd::rad2deg(getRollRad()); }
333 void setGroundTrackRad(double value)
334 { _groundTrackRad = value; _dirty = true; }
335 double getGroundTrackRad() const
336 { updateGeodeticFromCartesian(); return _groundTrackRad; }
337 void setGroundTrackDeg(double value)
338 { setGroundTrackRad(SGMiscd::deg2rad(value)); }
339 double getGroundTrackDeg() const
340 { return SGMiscd::rad2deg(getGroundTrackRad()); }
343 void setGroundSpeedMPerSec(double value)
344 { _groundSpeedMPerSec = value; _dirty = true; }
345 double getGroundSpeedMPerSec() const
346 { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
347 void setGroundSpeedFtPerSec(double value)
348 { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
349 double getGroundSpeedFtPerSec() const
350 { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
351 void setGroundSpeedKnots(double value)
352 { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
353 double getGroundSpeedKnots() const
354 { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
356 void setVerticalSpeedMPerSec(double value)
357 { _verticalSpeedMPerSec = value; _dirty = true; }
358 double getVerticalSpeedMPerSec() const
359 { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
360 void setVerticalSpeedFtPerSec(double value)
361 { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
362 double getVerticalSpeedFtPerSec() const
363 { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
364 void setVerticalSpeedFtPerMin(double value)
365 { setVerticalSpeedFtPerSec(value/60); }
366 double getVerticalSpeedFtPerMin() const
367 { return 60*getVerticalSpeedFtPerSec(); }
369 #define DATA_ELEMENT(name) \
370 HLADataElement* get## name ## DataElement() \
371 { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
373 DATA_ELEMENT(LatitudeDeg)
374 DATA_ELEMENT(LatitudeRad)
375 DATA_ELEMENT(LongitudeDeg)
376 DATA_ELEMENT(LongitudeRad)
377 DATA_ELEMENT(ElevationFt)
378 DATA_ELEMENT(ElevationM)
379 DATA_ELEMENT(HeadingDeg)
380 DATA_ELEMENT(HeadingRad)
381 DATA_ELEMENT(PitchDeg)
382 DATA_ELEMENT(PitchRad)
383 DATA_ELEMENT(RollDeg)
384 DATA_ELEMENT(RollRad)
386 DATA_ELEMENT(GroundTrackDeg)
387 DATA_ELEMENT(GroundTrackRad)
388 DATA_ELEMENT(GroundSpeedMPerSec)
389 DATA_ELEMENT(GroundSpeedFtPerSec)
390 DATA_ELEMENT(GroundSpeedKnots)
391 DATA_ELEMENT(VerticalSpeedMPerSec)
392 DATA_ELEMENT(VerticalSpeedFtPerSec)
393 DATA_ELEMENT(VerticalSpeedFtPerMin)
397 HLADataElement* getDataElement(Semantic semantic)
401 return getLatitudeDegDataElement();
403 return getLatitudeRadDataElement();
405 return getLongitudeDegDataElement();
407 return getLongitudeRadDataElement();
409 return getElevationMDataElement();
411 return getElevationFtDataElement();
413 return getHeadingDegDataElement();
415 return getHeadingRadDataElement();
417 return getPitchDegDataElement();
419 return getPitchRadDataElement();
421 return getRollDegDataElement();
423 return getRollRadDataElement();
425 return getGroundTrackDegDataElement();
427 return getGroundTrackRadDataElement();
428 case GroundSpeedKnots:
429 return getGroundSpeedKnotsDataElement();
430 case GroundSpeedFtPerSec:
431 return getGroundSpeedFtPerSecDataElement();
432 case GroundSpeedMPerSec:
433 return getGroundSpeedMPerSecDataElement();
434 case VerticalSpeedFtPerSec:
435 return getVerticalSpeedFtPerSecDataElement();
436 case VerticalSpeedFtPerMin:
437 return getVerticalSpeedFtPerMinDataElement();
438 case VerticalSpeedMPerSec:
439 return getVerticalSpeedMPerSecDataElement();
446 template<double (HLAGeodeticLocation::*getter)() const,
447 void (HLAGeodeticLocation::*setter)(double)>
448 class DataElement : public HLAAbstractDoubleDataElement {
450 DataElement(HLAGeodeticLocation* data) :
453 virtual double getValue() const
454 { return (_data->*getter)(); }
455 virtual void setValue(double value)
456 { (_data->*setter)(value); }
459 SGSharedPtr<HLAGeodeticLocation> _data;
462 void updateGeodeticFromCartesian() const
466 _geodPosition = SGGeod::fromCart(_cartPosition);
467 SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
468 geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
469 SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
470 if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
471 _groundTrackRad = atan2(nedVel[1], nedVel[0]);
474 _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
475 _verticalSpeedMPerSec = -nedVel[2];
478 void updateCartesianFromGeodetic() const
482 _cartPosition = SGVec3d::fromGeod(_geodPosition);
483 SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
484 _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
485 SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
486 sin(_groundTrackRad)*_groundSpeedMPerSec,
487 -_verticalSpeedMPerSec);
488 _cartBodyVelocity = geodOrientation.transform(nedVel);
494 // the cartesian values
495 mutable SGVec3d _cartPosition;
496 mutable SGQuatd _cartOrientation;
497 mutable SGVec3d _cartBodyVelocity;
499 // The geodetic values
500 mutable SGGeod _geodPosition;
501 mutable SGVec3d _geodEulerRad;
502 mutable double _groundTrackRad;
503 mutable double _groundSpeedMPerSec;
504 mutable double _verticalSpeedMPerSec;
507 class HLAGeodeticLocationFactory : public HLALocationFactory {
510 LatitudeDeg = HLAGeodeticLocation::LatitudeDeg,
511 LatitudeRad = HLAGeodeticLocation::LatitudeRad,
512 LongitudeDeg = HLAGeodeticLocation::LongitudeDeg,
513 LongitudeRad = HLAGeodeticLocation::LongitudeRad,
514 ElevationM = HLAGeodeticLocation::ElevationM,
515 ElevationFt = HLAGeodeticLocation::ElevationFt,
516 HeadingDeg = HLAGeodeticLocation::HeadingDeg,
517 HeadingRad = HLAGeodeticLocation::HeadingRad,
518 PitchDeg = HLAGeodeticLocation::PitchDeg,
519 PitchRad = HLAGeodeticLocation::PitchRad,
520 RollDeg = HLAGeodeticLocation::RollDeg,
521 RollRad = HLAGeodeticLocation::RollRad,
522 GroundTrackDeg = HLAGeodeticLocation::GroundTrackDeg,
523 GroundTrackRad = HLAGeodeticLocation::GroundTrackRad,
524 GroundSpeedKnots = HLAGeodeticLocation::GroundSpeedKnots,
525 GroundSpeedFtPerSec = HLAGeodeticLocation::GroundSpeedFtPerSec,
526 GroundSpeedMPerSec = HLAGeodeticLocation::GroundSpeedMPerSec,
527 VerticalSpeedFtPerSec = HLAGeodeticLocation::VerticalSpeedFtPerSec,
528 VerticalSpeedFtPerMin = HLAGeodeticLocation::VerticalSpeedFtPerMin,
529 VerticalSpeedMPerSec = HLAGeodeticLocation::VerticalSpeedMPerSec
532 virtual HLAGeodeticLocation* createLocation(HLAObjectInstance& objectInstance) const
534 HLAGeodeticLocation* location = new HLAGeodeticLocation;
536 for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
537 i != _indexSemanticMap.end(); ++i) {
538 HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
539 objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
545 void setIndex(Semantic semantic, const HLADataElementIndex& index)
546 { _indexSemanticMap[index] = semantic; }
549 typedef std::map<HLADataElementIndex, Semantic> IndexSemanticMap;
550 IndexSemanticMap _indexSemanticMap;
553 } // namespace simgear