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 #include "HLATypes.hxx"
  23
  24 namespace simgear {
  25
  26 class HLAAbstractLocation : public SGReferenced {
  27 public:
  28     virtual ~HLAAbstractLocation() {}
  29
  30     virtual SGVec3d getCartPosition() const = 0;
  31     virtual void setCartPosition(const SGVec3d&) = 0;
  32
  33     virtual SGQuatd getCartOrientation() const = 0;
  34     virtual void setCartOrientation(const SGQuatd&) = 0;
  35
  36     virtual SGVec3d getAngularBodyVelocity() const = 0;
  37     virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
  38
  39     virtual SGVec3d getLinearBodyVelocity() const = 0;
  40     virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
  41 };
  42
  43 class HLACartesianLocation : public HLAAbstractLocation {
  44 public:
  45     HLACartesianLocation() :
  46         _position(SGVec3d::zeros()),
  47         _imag(SGVec3d::zeros()),
  48         _angularVelocity(SGVec3d::zeros()),
  49         _linearVelocity(SGVec3d::zeros())
  50     { }
  51
  52     virtual SGVec3d getCartPosition() const
  53     { return _position; }
  54     virtual void setCartPosition(const SGVec3d& position)
  55     { _position = position; }
  56
  57     virtual SGQuatd getCartOrientation() const
  58     { return SGQuatd::fromPositiveRealImag(_imag); }
  59     virtual void setCartOrientation(const SGQuatd& orientation)
  60     { _imag = orientation.getPositiveRealImag(); }
  61
  62     virtual SGVec3d getAngularBodyVelocity() const
  63     { return _angularVelocity; }
  64     virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
  65     { _angularVelocity = angularVelocity; }
  66
  67     virtual SGVec3d getLinearBodyVelocity() const
  68     { return _linearVelocity; }
  69     virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
  70     { _linearVelocity = linearVelocity; }
  71
  72     HLADataElement* getPositionDataElement(unsigned i)
  73     {
  74         if (3 <= i)
  75             return 0;
  76         return new PositionDataElement(this, i);
  77     }
  78     HLADataElement* getOrientationDataElement(unsigned i)
  79     {
  80         if (3 <= i)
  81             return 0;
  82         return new OrientationDataElement(this, i);
  83     }
  84
  85     HLADataElement* getAngularVelocityDataElement(unsigned i)
  86     {
  87         if (3 <= i)
  88             return 0;
  89         return new AngularVelocityDataElement(this, i);
  90     }
  91     HLADataElement* getLinearVelocityDataElement(unsigned i)
  92     {
  93         if (3 <= i)
  94             return 0;
  95         return new LinearVelocityDataElement(this, i);
  96     }
  97
  98 private:
  99     class PositionDataElement : public HLAAbstractDoubleDataElement {
 100     public:
 101         PositionDataElement(HLACartesianLocation* data, unsigned index) :
 102             _data(data), _index(index)
 103         { }
 104         virtual double getValue() const
 105         { return _data->_position[_index]; }
 106         virtual void setValue(double value)
 107         { _data->_position[_index] = value; }
 108
 109     private:
 110         SGSharedPtr<HLACartesianLocation> _data;
 111         unsigned _index;
 112     };
 113
 114     class OrientationDataElement : public HLAAbstractDoubleDataElement {
 115     public:
 116         OrientationDataElement(HLACartesianLocation* data, unsigned index) :
 117             _data(data), _index(index)
 118         { }
 119         virtual double getValue() const
 120         { return _data->_imag[_index]; }
 121         virtual void setValue(double value)
 122         { _data->_imag[_index] = value; }
 123
 124     private:
 125         SGSharedPtr<HLACartesianLocation> _data;
 126         unsigned _index;
 127     };
 128
 129     class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
 130     public:
 131         AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
 132             _data(data), _index(index)
 133         { }
 134         virtual double getValue() const
 135         { return _data->_angularVelocity[_index]; }
 136         virtual void setValue(double value)
 137         { _data->_angularVelocity[_index] = value; }
 138
 139     private:
 140         SGSharedPtr<HLACartesianLocation> _data;
 141         unsigned _index;
 142     };
 143
 144     class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
 145     public:
 146         LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
 147             _data(data), _index(index)
 148         { }
 149         virtual double getValue() const
 150         { return _data->_linearVelocity[_index]; }
 151         virtual void setValue(double value)
 152         { _data->_linearVelocity[_index] = value; }
 153
 154     private:
 155         SGSharedPtr<HLACartesianLocation> _data;
 156         unsigned _index;
 157     };
 158
 159     SGVec3d _position;
 160     SGVec3d _imag;
 161
 162     SGVec3d _angularVelocity;
 163     SGVec3d _linearVelocity;
 164 };
 165
 166 class HLALocationFactory : public SGReferenced {
 167 public:
 168     virtual ~HLALocationFactory() {}
 169     virtual HLAAbstractLocation* createLocation(HLAObjectInstance&) const = 0;
 170 };
 171
 172 class HLACartesianLocationFactory : public HLALocationFactory {
 173 public:
 174     virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const
 175     {
 176         HLACartesianLocation* location = new HLACartesianLocation;
 177
 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));
 186
 187         return location;
 188     }
 189
 190     void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 191     {
 192         if (3 <= index)
 193             return;
 194         _positonIndex[index] = dataElementIndex;
 195     }
 196     void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 197     {
 198         if (3 <= index)
 199             return;
 200         _orientationIndex[index] = dataElementIndex;
 201     }
 202
 203     void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 204     {
 205         if (3 <= index)
 206             return;
 207         _angularVelocityIndex[index] = dataElementIndex;
 208     }
 209     void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 210     {
 211         if (3 <= index)
 212             return;
 213         _linearVelocityIndex[index] = dataElementIndex;
 214     }
 215
 216 private:
 217     HLADataElementIndex _positonIndex[3];
 218     HLADataElementIndex _orientationIndex[3];
 219
 220     HLADataElementIndex _angularVelocityIndex[3];
 221     HLADataElementIndex _linearVelocityIndex[3];
 222 };
 223
 224 class HLAGeodeticLocation : public HLAAbstractLocation {
 225 public:
 226     enum Semantic {
 227         LatitudeDeg,
 228         LatitudeRad,
 229         LongitudeDeg,
 230         LongitudeRad,
 231         ElevationM,
 232         ElevationFt,
 233         HeadingDeg,
 234         HeadingRad,
 235         PitchDeg,
 236         PitchRad,
 237         RollDeg,
 238         RollRad,
 239         GroundTrackDeg,
 240         GroundTrackRad,
 241         GroundSpeedKnots,
 242         GroundSpeedFtPerSec,
 243         GroundSpeedMPerSec,
 244         VerticalSpeedFtPerSec,
 245         VerticalSpeedFtPerMin,
 246         VerticalSpeedMPerSec
 247     };
 248
 249     HLAGeodeticLocation() :
 250         _dirty(true),
 251         _cartPosition(SGVec3d::zeros()),
 252         _cartOrientation(SGQuatd::unit()),
 253         _cartBodyVelocity(SGVec3d::zeros()),
 254         _geodPosition(),
 255         _geodEulerRad(SGVec3d::zeros()),
 256         _groundTrackRad(0),
 257         _groundSpeedMPerSec(0),
 258         _verticalSpeedMPerSec(0)
 259     {
 260         updateCartesianFromGeodetic();
 261     }
 262
 263     virtual SGVec3d getCartPosition() const
 264     { updateCartesianFromGeodetic(); return _cartPosition; }
 265     virtual void setCartPosition(const SGVec3d& position)
 266     { _cartPosition = position; _dirty = true; }
 267
 268     virtual SGQuatd getCartOrientation() const
 269     { updateCartesianFromGeodetic(); return _cartOrientation; }
 270     virtual void setCartOrientation(const SGQuatd& orientation)
 271     { _cartOrientation = orientation; _dirty = true; }
 272
 273     virtual SGVec3d getAngularBodyVelocity() const
 274     { return SGVec3d::zeros(); }
 275     virtual void setAngularBodyVelocity(const SGVec3d& angular)
 276     { }
 277
 278     virtual SGVec3d getLinearBodyVelocity() const
 279     { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
 280     virtual void setLinearBodyVelocity(const SGVec3d& linear)
 281     { _cartBodyVelocity = linear; _dirty = true; }
 282
 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(); }
 307
 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()); }
 332
 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()); }
 341
 342
 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(); }
 355
 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(); }
 368
 369 #define DATA_ELEMENT(name) \
 370     HLADataElement* get## name ## DataElement()                  \
 371     { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
 372
 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)
 385
 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)
 394
 395 #undef DATA_ELEMENT
 396
 397     HLADataElement* getDataElement(Semantic semantic)
 398     {
 399         switch (semantic) {
 400         case LatitudeDeg:
 401             return getLatitudeDegDataElement();
 402         case LatitudeRad:
 403             return getLatitudeRadDataElement();
 404         case LongitudeDeg:
 405             return getLongitudeDegDataElement();
 406         case LongitudeRad:
 407             return getLongitudeRadDataElement();
 408         case ElevationM:
 409             return getElevationMDataElement();
 410         case ElevationFt:
 411             return getElevationFtDataElement();
 412         case HeadingDeg:
 413             return getHeadingDegDataElement();
 414         case HeadingRad:
 415             return getHeadingRadDataElement();
 416         case PitchDeg:
 417             return getPitchDegDataElement();
 418         case PitchRad:
 419             return getPitchRadDataElement();
 420         case RollDeg:
 421             return getRollDegDataElement();
 422         case RollRad:
 423             return getRollRadDataElement();
 424         case GroundTrackDeg:
 425             return getGroundTrackDegDataElement();
 426         case GroundTrackRad:
 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();
 440         default:
 441             return 0;
 442         }
 443     }
 444
 445 private:
 446     template<double (HLAGeodeticLocation::*getter)() const,
 447              void (HLAGeodeticLocation::*setter)(double)>
 448     class DataElement : public HLAAbstractDoubleDataElement {
 449     public:
 450         DataElement(HLAGeodeticLocation* data) :
 451             _data(data)
 452         { }
 453         virtual double getValue() const
 454         { return (_data->*getter)(); }
 455         virtual void setValue(double value)
 456         { (_data->*setter)(value); }
 457
 458     private:
 459         SGSharedPtr<HLAGeodeticLocation> _data;
 460     };
 461
 462     void updateGeodeticFromCartesian() const
 463     {
 464         if (!_dirty)
 465             return;
 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]);
 472         else
 473             _groundTrackRad = 0;
 474         _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
 475         _verticalSpeedMPerSec = -nedVel[2];
 476         _dirty = false;
 477     }
 478     void updateCartesianFromGeodetic() const
 479     {
 480         if (!_dirty)
 481             return;
 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);
 489         _dirty = false;
 490     }
 491
 492     mutable bool _dirty;
 493
 494     // the cartesian values
 495     mutable SGVec3d _cartPosition;
 496     mutable SGQuatd _cartOrientation;
 497     mutable SGVec3d _cartBodyVelocity;
 498
 499     // The geodetic values
 500     mutable SGGeod _geodPosition;
 501     mutable SGVec3d _geodEulerRad;
 502     mutable double _groundTrackRad;
 503     mutable double _groundSpeedMPerSec;
 504     mutable double _verticalSpeedMPerSec;
 505 };
 506
 507 class HLAGeodeticLocationFactory : public HLALocationFactory {
 508 public:
 509     enum Semantic {
 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
 530     };
 531
 532     virtual HLAGeodeticLocation* createLocation(HLAObjectInstance& objectInstance) const
 533     {
 534         HLAGeodeticLocation* location = new HLAGeodeticLocation;
 535
 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));
 540         }
 541
 542         return location;
 543     }
 544
 545     void setIndex(Semantic semantic, const HLADataElementIndex& index)
 546     { _indexSemanticMap[index] = semantic; }
 547
 548 private:
 549     typedef std::map<HLADataElementIndex, Semantic> IndexSemanticMap;
 550     IndexSemanticMap _indexSemanticMap;
 551 };
 552
 553 } // namespace simgear
 554
 555 #endif