]> git.mxchange.org Git - simgear.git/blob - simgear/hla/HLALocation.hxx
Add an initial implementation of a rti/hla dispatcher.
[simgear.git] / simgear / hla / HLALocation.hxx
1 // Copyright (C) 2009 - 2010  Mathias Froehlich - Mathias.Froehlich@web.de
2 //
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.
7 //
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.
12 //
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.
16 //
17
18 #ifndef HLALocation_hxx
19 #define HLALocation_hxx
20
21 #include "HLABasicDataElement.hxx"
22
23 namespace simgear {
24
25 class HLAAbstractLocation : public SGReferenced {
26 public:
27     virtual ~HLAAbstractLocation() {}
28
29     virtual SGVec3d getCartPosition() const = 0;
30     virtual void setCartPosition(const SGVec3d&) = 0;
31
32     virtual SGQuatd getCartOrientation() const = 0;
33     virtual void setCartOrientation(const SGQuatd&) = 0;
34
35     virtual SGVec3d getAngularBodyVelocity() const = 0;
36     virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
37
38     virtual SGVec3d getLinearBodyVelocity() const = 0;
39     virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
40 };
41
42 class HLACartesianLocation : public HLAAbstractLocation {
43 public:
44     HLACartesianLocation() :
45         _position(SGVec3d::zeros()),
46         _imag(SGVec3d::zeros()),
47         _angularVelocity(SGVec3d::zeros()),
48         _linearVelocity(SGVec3d::zeros())
49     { }
50
51     virtual SGVec3d getCartPosition() const
52     { return _position; }
53     virtual void setCartPosition(const SGVec3d& position)
54     { _position = position; }
55
56     virtual SGQuatd getCartOrientation() const
57     { return SGQuatd::fromPositiveRealImag(_imag); }
58     virtual void setCartOrientation(const SGQuatd& orientation)
59     { _imag = orientation.getPositiveRealImag(); }
60
61     virtual SGVec3d getAngularBodyVelocity() const
62     { return _angularVelocity; }
63     virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
64     { _angularVelocity = angularVelocity; }
65
66     virtual SGVec3d getLinearBodyVelocity() const
67     { return _linearVelocity; }
68     virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
69     { _linearVelocity = linearVelocity; }
70
71     HLADataElementProvider getPositionDataElement(unsigned i)
72     {
73         if (3 <= i)
74             return HLADataElementProvider();
75         return new PositionDataElement(this, i);
76     }
77     HLADataElementProvider getOrientationDataElement(unsigned i)
78     {
79         if (3 <= i)
80             return HLADataElementProvider();
81         return new OrientationDataElement(this, i);
82     }
83
84     HLADataElementProvider getAngularVelocityDataElement(unsigned i)
85     {
86         if (3 <= i)
87             return HLADataElementProvider();
88         return new AngularVelocityDataElement(this, i);
89     }
90     HLADataElementProvider getLinearVelocityDataElement(unsigned i)
91     {
92         if (3 <= i)
93             return HLADataElementProvider();
94         return new LinearVelocityDataElement(this, i);
95     }
96
97 private:
98     class PositionDataElement : public HLAAbstractDoubleDataElement {
99     public:
100         PositionDataElement(HLACartesianLocation* data, unsigned index) :
101             _data(data), _index(index)
102         { }
103         virtual double getValue() const
104         { return _data->_position[_index]; }
105         virtual void setValue(double value)
106         { _data->_position[_index] = value; }
107
108     private:
109         SGSharedPtr<HLACartesianLocation> _data;
110         unsigned _index;
111     };
112
113     class OrientationDataElement : public HLAAbstractDoubleDataElement {
114     public:
115         OrientationDataElement(HLACartesianLocation* data, unsigned index) :
116             _data(data), _index(index)
117         { }
118         virtual double getValue() const
119         { return _data->_imag[_index]; }
120         virtual void setValue(double value)
121         { _data->_imag[_index] = value; }
122
123     private:
124         SGSharedPtr<HLACartesianLocation> _data;
125         unsigned _index;
126     };
127
128     class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
129     public:
130         AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
131             _data(data), _index(index)
132         { }
133         virtual double getValue() const
134         { return _data->_angularVelocity[_index]; }
135         virtual void setValue(double value)
136         { _data->_angularVelocity[_index] = value; }
137
138     private:
139         SGSharedPtr<HLACartesianLocation> _data;
140         unsigned _index;
141     };
142
143     class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
144     public:
145         LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
146             _data(data), _index(index)
147         { }
148         virtual double getValue() const
149         { return _data->_linearVelocity[_index]; }
150         virtual void setValue(double value)
151         { _data->_linearVelocity[_index] = value; }
152
153     private:
154         SGSharedPtr<HLACartesianLocation> _data;
155         unsigned _index;
156     };
157
158     SGVec3d _position;
159     SGVec3d _imag;
160
161     SGVec3d _angularVelocity;
162     SGVec3d _linearVelocity;
163 };
164
165 class HLALocationFactory : public SGReferenced {
166 public:
167     virtual ~HLALocationFactory() {}
168     virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap&) const = 0;
169 };
170
171 class HLACartesianLocationFactory : public HLALocationFactory {
172 public:
173     virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
174     {
175         HLACartesianLocation* location = new HLACartesianLocation;
176         for (unsigned i = 0; i < 3; ++i) {
177             const HLADataElement::IndexPathPair& indexPathPair = _positonIndexPathPair[i];
178             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getPositionDataElement(i);
179         }
180         for (unsigned i = 0; i < 3; ++i) {
181             const HLADataElement::IndexPathPair& indexPathPair = _orientationIndexPathPair[i];
182             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getOrientationDataElement(i);
183         }
184         for (unsigned i = 0; i < 3; ++i) {
185             const HLADataElement::IndexPathPair& indexPathPair = _angularVelocityIndexPathPair[i];
186             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getAngularVelocityDataElement(i);
187         }
188         for (unsigned i = 0; i < 3; ++i) {
189             const HLADataElement::IndexPathPair& indexPathPair = _linearVelocityIndexPathPair[i];
190             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getLinearVelocityDataElement(i);
191         }
192         return location;
193     }
194
195     void setPositionIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
196     {
197         if (3 <= index)
198             return;
199         _positonIndexPathPair[index] = indexPathPair;
200     }
201     void setOrientationIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
202     {
203         if (3 <= index)
204             return;
205         _orientationIndexPathPair[index] = indexPathPair;
206     }
207
208     void setAngularVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
209     {
210         if (3 <= index)
211             return;
212         _angularVelocityIndexPathPair[index] = indexPathPair;
213     }
214     void setLinearVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
215     {
216         if (3 <= index)
217             return;
218         _linearVelocityIndexPathPair[index] = indexPathPair;
219     }
220
221 private:
222     HLADataElement::IndexPathPair _positonIndexPathPair[3];
223     HLADataElement::IndexPathPair _orientationIndexPathPair[3];
224
225     HLADataElement::IndexPathPair _angularVelocityIndexPathPair[3];
226     HLADataElement::IndexPathPair _linearVelocityIndexPathPair[3];
227 };
228
229 class HLAGeodeticLocation : public HLAAbstractLocation {
230 public:
231     HLAGeodeticLocation() :
232         _dirty(true),
233         _cartPosition(SGVec3d::zeros()),
234         _cartOrientation(SGQuatd::unit()),
235         _cartBodyVelocity(SGVec3d::zeros()),
236         _geodPosition(),
237         _geodEulerRad(SGVec3d::zeros()),
238         _groundTrackRad(0),
239         _groundSpeedMPerSec(0),
240         _verticalSpeedMPerSec(0)
241     {
242         updateCartesianFromGeodetic();
243     }
244
245     virtual SGVec3d getCartPosition() const
246     { updateCartesianFromGeodetic(); return _cartPosition; }
247     virtual void setCartPosition(const SGVec3d& position)
248     { _cartPosition = position; _dirty = true; }
249
250     virtual SGQuatd getCartOrientation() const
251     { updateCartesianFromGeodetic(); return _cartOrientation; }
252     virtual void setCartOrientation(const SGQuatd& orientation)
253     { _cartOrientation = orientation; _dirty = true; }
254
255     virtual SGVec3d getAngularBodyVelocity() const
256     { return SGVec3d::zeros(); }
257     virtual void setAngularBodyVelocity(const SGVec3d& angular)
258     { }
259
260     virtual SGVec3d getLinearBodyVelocity() const
261     { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
262     virtual void setLinearBodyVelocity(const SGVec3d& linear)
263     { _cartBodyVelocity = linear; _dirty = true; }
264
265     void setLatitudeDeg(double value)
266     { _geodPosition.setLatitudeDeg(value); _dirty = true; }
267     double getLatitudeDeg() const
268     { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
269     void setLatitudeRad(double value)
270     { _geodPosition.setLatitudeRad(value); _dirty = true; }
271     double getLatitudeRad() const
272     { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
273     void setLongitudeDeg(double value)
274     { _geodPosition.setLongitudeDeg(value); _dirty = true; }
275     double getLongitudeDeg() const
276     { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
277     void setLongitudeRad(double value)
278     { _geodPosition.setLongitudeRad(value); _dirty = true; }
279     double getLongitudeRad() const
280     { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
281     void setElevationFt(double value)
282     { _geodPosition.setElevationFt(value); _dirty = true; }
283     double getElevationFt() const
284     { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
285     void setElevationM(double value)
286     { _geodPosition.setElevationM(value); _dirty = true; }
287     double getElevationM() const
288     { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
289
290     void setHeadingRad(double value)
291     { _geodEulerRad[2] = value; _dirty = true; }
292     double getHeadingRad() const
293     { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
294     void setHeadingDeg(double value)
295     { setHeadingRad(SGMiscd::deg2rad(value)); }
296     double getHeadingDeg() const
297     { return SGMiscd::rad2deg(getHeadingRad()); }
298     void setPitchRad(double value)
299     { _geodEulerRad[1] = value; _dirty = true; }
300     double getPitchRad() const
301     { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
302     void setPitchDeg(double value)
303     { setPitchRad(SGMiscd::deg2rad(value)); }
304     double getPitchDeg() const
305     { return SGMiscd::rad2deg(getPitchRad()); }
306     void setRollRad(double value)
307     { _geodEulerRad[0] = value; _dirty = true; }
308     double getRollRad() const
309     { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
310     void setRollDeg(double value)
311     { setRollRad(SGMiscd::deg2rad(value)); }
312     double getRollDeg() const
313     { return SGMiscd::rad2deg(getRollRad()); }
314
315     void setGroundTrackRad(double value)
316     { _groundTrackRad = value; _dirty = true; }
317     double getGroundTrackRad() const
318     { updateGeodeticFromCartesian(); return _groundTrackRad; }
319     void setGroundTrackDeg(double value)
320     { setGroundTrackRad(SGMiscd::deg2rad(value)); }
321     double getGroundTrackDeg() const
322     { return SGMiscd::rad2deg(getGroundTrackRad()); }
323
324
325     void setGroundSpeedMPerSec(double value)
326     { _groundSpeedMPerSec = value; _dirty = true; }
327     double getGroundSpeedMPerSec() const
328     { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
329     void setGroundSpeedFtPerSec(double value)
330     { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
331     double getGroundSpeedFtPerSec() const
332     { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
333     void setGroundSpeedKnots(double value)
334     { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
335     double getGroundSpeedKnots() const
336     { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
337
338     void setVerticalSpeedMPerSec(double value)
339     { _verticalSpeedMPerSec = value; _dirty = true; }
340     double getVerticalSpeedMPerSec() const
341     { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
342     void setVerticalSpeedFtPerSec(double value)
343     { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
344     double getVerticalSpeedFtPerSec() const
345     { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
346     void setVerticalSpeedFtPerMin(double value)
347     { setVerticalSpeedFtPerSec(value/60); }
348     double getVerticalSpeedFtPerMin() const
349     { return 60*getVerticalSpeedFtPerSec(); }
350
351 #define DATA_ELEMENT(name) \
352     HLADataElementProvider get## name ## DataElement()                  \
353     { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
354
355     DATA_ELEMENT(LatitudeDeg)
356     DATA_ELEMENT(LatitudeRad)
357     DATA_ELEMENT(LongitudeDeg)
358     DATA_ELEMENT(LongitudeRad)
359     DATA_ELEMENT(ElevationFt)
360     DATA_ELEMENT(ElevationM)
361     DATA_ELEMENT(HeadingDeg)
362     DATA_ELEMENT(HeadingRad)
363     DATA_ELEMENT(PitchDeg)
364     DATA_ELEMENT(PitchRad)
365     DATA_ELEMENT(RollDeg)
366     DATA_ELEMENT(RollRad)
367
368     DATA_ELEMENT(GroundTrackDeg)
369     DATA_ELEMENT(GroundTrackRad)
370     DATA_ELEMENT(GroundSpeedMPerSec)
371     DATA_ELEMENT(GroundSpeedFtPerSec)
372     DATA_ELEMENT(GroundSpeedKnots)
373     DATA_ELEMENT(VerticalSpeedMPerSec)
374     DATA_ELEMENT(VerticalSpeedFtPerSec)
375     DATA_ELEMENT(VerticalSpeedFtPerMin)
376
377 #undef DATA_ELEMENT
378
379 private:
380     template<double (HLAGeodeticLocation::*getter)() const,
381              void (HLAGeodeticLocation::*setter)(double)>
382     class DataElement : public HLAAbstractDoubleDataElement {
383     public:
384         DataElement(HLAGeodeticLocation* data) :
385             _data(data)
386         { }
387         virtual double getValue() const
388         { return (_data->*getter)(); }
389         virtual void setValue(double value)
390         { (_data->*setter)(value); }
391
392     private:
393         SGSharedPtr<HLAGeodeticLocation> _data;
394     };
395
396     void updateGeodeticFromCartesian() const
397     {
398         if (!_dirty)
399             return;
400         _geodPosition = SGGeod::fromCart(_cartPosition);
401         SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
402         geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
403         SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
404         if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
405             _groundTrackRad = atan2(nedVel[1], nedVel[0]);
406         else
407             _groundTrackRad = 0;
408         _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
409         _verticalSpeedMPerSec = -nedVel[2];
410         _dirty = false;
411     }
412     void updateCartesianFromGeodetic() const
413     {
414         if (!_dirty)
415             return;
416         _cartPosition = SGVec3d::fromGeod(_geodPosition);
417         SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
418         _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
419         SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
420                        sin(_groundTrackRad)*_groundSpeedMPerSec,
421                        -_verticalSpeedMPerSec);
422         _cartBodyVelocity = geodOrientation.transform(nedVel);
423         _dirty = false;
424     }
425
426     mutable bool _dirty;
427
428     // the cartesian values
429     mutable SGVec3d _cartPosition;
430     mutable SGQuatd _cartOrientation;
431     mutable SGVec3d _cartBodyVelocity;
432
433     // The geodetic values
434     mutable SGGeod _geodPosition;
435     mutable SGVec3d _geodEulerRad;
436     mutable double _groundTrackRad;
437     mutable double _groundSpeedMPerSec;
438     mutable double _verticalSpeedMPerSec;
439 };
440
441 class HLAGeodeticLocationFactory : public HLALocationFactory {
442 public:
443     enum Semantic {
444         LatitudeDeg,
445         LatitudeRad,
446         LongitudeDeg,
447         LongitudeRad,
448         ElevationM,
449         ElevationFt,
450         HeadingDeg,
451         HeadingRad,
452         PitchDeg,
453         PitchRad,
454         RollDeg,
455         RollRad,
456         GroundTrackDeg,
457         GroundTrackRad,
458         GroundSpeedKnots,
459         GroundSpeedFtPerSec,
460         GroundSpeedMPerSec,
461         VerticalSpeedFtPerSec,
462         VerticalSpeedFtPerMin,
463         VerticalSpeedMPerSec
464     };
465
466     virtual HLAGeodeticLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
467     {
468         HLAGeodeticLocation* location = new HLAGeodeticLocation;
469
470         for (IndexPathPairSemanticMap::const_iterator i = _indexPathPairSemanticMap.begin();
471              i != _indexPathPairSemanticMap.end(); ++i) {
472             switch (i->second) {
473             case LatitudeDeg:
474                 attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeDegDataElement();
475                 break;
476             case LatitudeRad:
477                 attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeRadDataElement();
478                 break;
479             case LongitudeDeg:
480                 attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeDegDataElement();
481                 break;
482             case LongitudeRad:
483                 attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeRadDataElement();
484                 break;
485             case ElevationM:
486                 attributePathElementMap[i->first.first][i->first.second] = location->getElevationMDataElement();
487                 break;
488             case ElevationFt:
489                 attributePathElementMap[i->first.first][i->first.second] = location->getElevationFtDataElement();
490                 break;
491             case HeadingDeg:
492                 attributePathElementMap[i->first.first][i->first.second] = location->getHeadingDegDataElement();
493                 break;
494             case HeadingRad:
495                 attributePathElementMap[i->first.first][i->first.second] = location->getHeadingRadDataElement();
496                 break;
497             case PitchDeg:
498                 attributePathElementMap[i->first.first][i->first.second] = location->getPitchDegDataElement();
499                 break;
500             case PitchRad:
501                 attributePathElementMap[i->first.first][i->first.second] = location->getPitchRadDataElement();
502                 break;
503             case RollDeg:
504                 attributePathElementMap[i->first.first][i->first.second] = location->getRollDegDataElement();
505                 break;
506             case RollRad:
507                 attributePathElementMap[i->first.first][i->first.second] = location->getRollRadDataElement();
508                 break;
509             case GroundTrackDeg:
510                 attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackDegDataElement();
511                 break;
512             case GroundTrackRad:
513                 attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackRadDataElement();
514                 break;
515             case GroundSpeedKnots:
516                 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedKnotsDataElement();
517                 break;
518             case GroundSpeedFtPerSec:
519                 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedFtPerSecDataElement();
520                 break;
521             case GroundSpeedMPerSec:
522                 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedMPerSecDataElement();
523                 break;
524             case VerticalSpeedFtPerSec:
525                 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerSecDataElement();
526                 break;
527             case VerticalSpeedFtPerMin:
528                 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerMinDataElement();
529                 break;
530             case VerticalSpeedMPerSec:
531                 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedMPerSecDataElement();
532                 break;
533             }
534         }
535
536         return location;
537     }
538
539     void setIndexPathPair(Semantic semantic, const HLADataElement::IndexPathPair& indexPathPair)
540     { _indexPathPairSemanticMap[indexPathPair] = semantic; }
541
542 private:
543     typedef std::map<HLADataElement::IndexPathPair, Semantic> IndexPathPairSemanticMap;
544     IndexPathPairSemanticMap _indexPathPairSemanticMap;
545 };
546
547 } // namespace simgear
548
549 #endif