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(HLAAttributePathElementMap&) const = 0;
 170     virtual HLAAbstractLocation* createLocation(HLAObjectInstance&) const = 0;
 171 };
 172
 173 class HLACartesianLocationFactory : public HLALocationFactory {
 174 public:
 175     virtual HLACartesianLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
 176     {
 177         HLACartesianLocation* location = new HLACartesianLocation;
 178         for (unsigned i = 0; i < 3; ++i) {
 179             const HLADataElement::IndexPathPair& indexPathPair = _positonIndexPathPair[i];
 180             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getPositionDataElement(i);
 181         }
 182         for (unsigned i = 0; i < 3; ++i) {
 183             const HLADataElement::IndexPathPair& indexPathPair = _orientationIndexPathPair[i];
 184             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getOrientationDataElement(i);
 185         }
 186         for (unsigned i = 0; i < 3; ++i) {
 187             const HLADataElement::IndexPathPair& indexPathPair = _angularVelocityIndexPathPair[i];
 188             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getAngularVelocityDataElement(i);
 189         }
 190         for (unsigned i = 0; i < 3; ++i) {
 191             const HLADataElement::IndexPathPair& indexPathPair = _linearVelocityIndexPathPair[i];
 192             attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getLinearVelocityDataElement(i);
 193         }
 194         return location;
 195     }
 196
 197     virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const
 198     {
 199         HLACartesianLocation* location = new HLACartesianLocation;
 200         for (unsigned i = 0; i < 3; ++i) {
 201             const HLADataElement::IndexPathPair& indexPathPair = _positonIndexPathPair[i];
 202             if (indexPathPair.first == 0)
 203                 continue;
 204             std::string path = objectInstance.getAttributeName(indexPathPair.first);
 205             if (!indexPathPair.second.empty())
 206                 path += HLADataElement::toString(indexPathPair.second);
 207             HLADataElementIndex index;
 208             if (!objectInstance.getDataElementIndex(index, path))
 209                 continue;
 210             objectInstance.setAttributeDataElement(index, location->getPositionDataElement(i));
 211         }
 212         for (unsigned i = 0; i < 3; ++i) {
 213             const HLADataElement::IndexPathPair& indexPathPair = _orientationIndexPathPair[i];
 214             if (indexPathPair.first == 0)
 215                 continue;
 216             std::string path = objectInstance.getAttributeName(indexPathPair.first);
 217             if (!indexPathPair.second.empty())
 218                 path += HLADataElement::toString(indexPathPair.second);
 219             HLADataElementIndex index;
 220             if (!objectInstance.getDataElementIndex(index, path))
 221                 continue;
 222             objectInstance.setAttributeDataElement(index, location->getOrientationDataElement(i));
 223         }
 224         for (unsigned i = 0; i < 3; ++i) {
 225             const HLADataElement::IndexPathPair& indexPathPair = _angularVelocityIndexPathPair[i];
 226             if (indexPathPair.first == 0)
 227                 continue;
 228             std::string path = objectInstance.getAttributeName(indexPathPair.first);
 229             if (!indexPathPair.second.empty())
 230                 path += HLADataElement::toString(indexPathPair.second);
 231             HLADataElementIndex index;
 232             if (!objectInstance.getDataElementIndex(index, path))
 233                 continue;
 234             objectInstance.setAttributeDataElement(index, location->getAngularVelocityDataElement(i));
 235         }
 236         for (unsigned i = 0; i < 3; ++i) {
 237             const HLADataElement::IndexPathPair& indexPathPair = _linearVelocityIndexPathPair[i];
 238             if (indexPathPair.first == 0)
 239                 continue;
 240             std::string path = objectInstance.getAttributeName(indexPathPair.first);
 241             if (!indexPathPair.second.empty())
 242                 path += HLADataElement::toString(indexPathPair.second);
 243             HLADataElementIndex index;
 244             if (!objectInstance.getDataElementIndex(index, path))
 245                 continue;
 246             objectInstance.setAttributeDataElement(index, location->getLinearVelocityDataElement(i));
 247         }
 248
 249         for (unsigned i = 0; i < 3; ++i)
 250             objectInstance.setAttributeDataElement(_positonIndex[i], location->getPositionDataElement(i));
 251         for (unsigned i = 0; i < 3; ++i)
 252             objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
 253         for (unsigned i = 0; i < 3; ++i)
 254             objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
 255         for (unsigned i = 0; i < 3; ++i)
 256             objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
 257
 258         return location;
 259     }
 260
 261     void setPositionIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
 262     {
 263         if (3 <= index)
 264             return;
 265         _positonIndexPathPair[index] = indexPathPair;
 266     }
 267     void setOrientationIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
 268     {
 269         if (3 <= index)
 270             return;
 271         _orientationIndexPathPair[index] = indexPathPair;
 272     }
 273
 274     void setAngularVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
 275     {
 276         if (3 <= index)
 277             return;
 278         _angularVelocityIndexPathPair[index] = indexPathPair;
 279     }
 280     void setLinearVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
 281     {
 282         if (3 <= index)
 283             return;
 284         _linearVelocityIndexPathPair[index] = indexPathPair;
 285     }
 286
 287     void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 288     {
 289         if (3 <= index)
 290             return;
 291         _positonIndex[index] = dataElementIndex;
 292     }
 293     void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 294     {
 295         if (3 <= index)
 296             return;
 297         _orientationIndex[index] = dataElementIndex;
 298     }
 299
 300     void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 301     {
 302         if (3 <= index)
 303             return;
 304         _angularVelocityIndex[index] = dataElementIndex;
 305     }
 306     void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
 307     {
 308         if (3 <= index)
 309             return;
 310         _linearVelocityIndex[index] = dataElementIndex;
 311     }
 312
 313 private:
 314     HLADataElement::IndexPathPair _positonIndexPathPair[3];
 315     HLADataElement::IndexPathPair _orientationIndexPathPair[3];
 316
 317     HLADataElement::IndexPathPair _angularVelocityIndexPathPair[3];
 318     HLADataElement::IndexPathPair _linearVelocityIndexPathPair[3];
 319
 320     HLADataElementIndex _positonIndex[3];
 321     HLADataElementIndex _orientationIndex[3];
 322
 323     HLADataElementIndex _angularVelocityIndex[3];
 324     HLADataElementIndex _linearVelocityIndex[3];
 325 };
 326
 327 class HLAGeodeticLocation : public HLAAbstractLocation {
 328 public:
 329     enum Semantic {
 330         LatitudeDeg,
 331         LatitudeRad,
 332         LongitudeDeg,
 333         LongitudeRad,
 334         ElevationM,
 335         ElevationFt,
 336         HeadingDeg,
 337         HeadingRad,
 338         PitchDeg,
 339         PitchRad,
 340         RollDeg,
 341         RollRad,
 342         GroundTrackDeg,
 343         GroundTrackRad,
 344         GroundSpeedKnots,
 345         GroundSpeedFtPerSec,
 346         GroundSpeedMPerSec,
 347         VerticalSpeedFtPerSec,
 348         VerticalSpeedFtPerMin,
 349         VerticalSpeedMPerSec
 350     };
 351
 352     HLAGeodeticLocation() :
 353         _dirty(true),
 354         _cartPosition(SGVec3d::zeros()),
 355         _cartOrientation(SGQuatd::unit()),
 356         _cartBodyVelocity(SGVec3d::zeros()),
 357         _geodPosition(),
 358         _geodEulerRad(SGVec3d::zeros()),
 359         _groundTrackRad(0),
 360         _groundSpeedMPerSec(0),
 361         _verticalSpeedMPerSec(0)
 362     {
 363         updateCartesianFromGeodetic();
 364     }
 365
 366     virtual SGVec3d getCartPosition() const
 367     { updateCartesianFromGeodetic(); return _cartPosition; }
 368     virtual void setCartPosition(const SGVec3d& position)
 369     { _cartPosition = position; _dirty = true; }
 370
 371     virtual SGQuatd getCartOrientation() const
 372     { updateCartesianFromGeodetic(); return _cartOrientation; }
 373     virtual void setCartOrientation(const SGQuatd& orientation)
 374     { _cartOrientation = orientation; _dirty = true; }
 375
 376     virtual SGVec3d getAngularBodyVelocity() const
 377     { return SGVec3d::zeros(); }
 378     virtual void setAngularBodyVelocity(const SGVec3d& angular)
 379     { }
 380
 381     virtual SGVec3d getLinearBodyVelocity() const
 382     { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
 383     virtual void setLinearBodyVelocity(const SGVec3d& linear)
 384     { _cartBodyVelocity = linear; _dirty = true; }
 385
 386     void setLatitudeDeg(double value)
 387     { _geodPosition.setLatitudeDeg(value); _dirty = true; }
 388     double getLatitudeDeg() const
 389     { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
 390     void setLatitudeRad(double value)
 391     { _geodPosition.setLatitudeRad(value); _dirty = true; }
 392     double getLatitudeRad() const
 393     { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
 394     void setLongitudeDeg(double value)
 395     { _geodPosition.setLongitudeDeg(value); _dirty = true; }
 396     double getLongitudeDeg() const
 397     { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
 398     void setLongitudeRad(double value)
 399     { _geodPosition.setLongitudeRad(value); _dirty = true; }
 400     double getLongitudeRad() const
 401     { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
 402     void setElevationFt(double value)
 403     { _geodPosition.setElevationFt(value); _dirty = true; }
 404     double getElevationFt() const
 405     { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
 406     void setElevationM(double value)
 407     { _geodPosition.setElevationM(value); _dirty = true; }
 408     double getElevationM() const
 409     { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
 410
 411     void setHeadingRad(double value)
 412     { _geodEulerRad[2] = value; _dirty = true; }
 413     double getHeadingRad() const
 414     { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
 415     void setHeadingDeg(double value)
 416     { setHeadingRad(SGMiscd::deg2rad(value)); }
 417     double getHeadingDeg() const
 418     { return SGMiscd::rad2deg(getHeadingRad()); }
 419     void setPitchRad(double value)
 420     { _geodEulerRad[1] = value; _dirty = true; }
 421     double getPitchRad() const
 422     { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
 423     void setPitchDeg(double value)
 424     { setPitchRad(SGMiscd::deg2rad(value)); }
 425     double getPitchDeg() const
 426     { return SGMiscd::rad2deg(getPitchRad()); }
 427     void setRollRad(double value)
 428     { _geodEulerRad[0] = value; _dirty = true; }
 429     double getRollRad() const
 430     { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
 431     void setRollDeg(double value)
 432     { setRollRad(SGMiscd::deg2rad(value)); }
 433     double getRollDeg() const
 434     { return SGMiscd::rad2deg(getRollRad()); }
 435
 436     void setGroundTrackRad(double value)
 437     { _groundTrackRad = value; _dirty = true; }
 438     double getGroundTrackRad() const
 439     { updateGeodeticFromCartesian(); return _groundTrackRad; }
 440     void setGroundTrackDeg(double value)
 441     { setGroundTrackRad(SGMiscd::deg2rad(value)); }
 442     double getGroundTrackDeg() const
 443     { return SGMiscd::rad2deg(getGroundTrackRad()); }
 444
 445
 446     void setGroundSpeedMPerSec(double value)
 447     { _groundSpeedMPerSec = value; _dirty = true; }
 448     double getGroundSpeedMPerSec() const
 449     { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
 450     void setGroundSpeedFtPerSec(double value)
 451     { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
 452     double getGroundSpeedFtPerSec() const
 453     { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
 454     void setGroundSpeedKnots(double value)
 455     { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
 456     double getGroundSpeedKnots() const
 457     { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
 458
 459     void setVerticalSpeedMPerSec(double value)
 460     { _verticalSpeedMPerSec = value; _dirty = true; }
 461     double getVerticalSpeedMPerSec() const
 462     { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
 463     void setVerticalSpeedFtPerSec(double value)
 464     { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
 465     double getVerticalSpeedFtPerSec() const
 466     { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
 467     void setVerticalSpeedFtPerMin(double value)
 468     { setVerticalSpeedFtPerSec(value/60); }
 469     double getVerticalSpeedFtPerMin() const
 470     { return 60*getVerticalSpeedFtPerSec(); }
 471
 472 #define DATA_ELEMENT(name) \
 473     HLADataElement* get## name ## DataElement()                  \
 474     { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
 475
 476     DATA_ELEMENT(LatitudeDeg)
 477     DATA_ELEMENT(LatitudeRad)
 478     DATA_ELEMENT(LongitudeDeg)
 479     DATA_ELEMENT(LongitudeRad)
 480     DATA_ELEMENT(ElevationFt)
 481     DATA_ELEMENT(ElevationM)
 482     DATA_ELEMENT(HeadingDeg)
 483     DATA_ELEMENT(HeadingRad)
 484     DATA_ELEMENT(PitchDeg)
 485     DATA_ELEMENT(PitchRad)
 486     DATA_ELEMENT(RollDeg)
 487     DATA_ELEMENT(RollRad)
 488
 489     DATA_ELEMENT(GroundTrackDeg)
 490     DATA_ELEMENT(GroundTrackRad)
 491     DATA_ELEMENT(GroundSpeedMPerSec)
 492     DATA_ELEMENT(GroundSpeedFtPerSec)
 493     DATA_ELEMENT(GroundSpeedKnots)
 494     DATA_ELEMENT(VerticalSpeedMPerSec)
 495     DATA_ELEMENT(VerticalSpeedFtPerSec)
 496     DATA_ELEMENT(VerticalSpeedFtPerMin)
 497
 498 #undef DATA_ELEMENT
 499
 500     HLADataElement* getDataElement(Semantic semantic)
 501     {
 502         switch (semantic) {
 503         case LatitudeDeg:
 504             return getLatitudeDegDataElement();
 505         case LatitudeRad:
 506             return getLatitudeRadDataElement();
 507         case LongitudeDeg:
 508             return getLongitudeDegDataElement();
 509         case LongitudeRad:
 510             return getLongitudeRadDataElement();
 511         case ElevationM:
 512             return getElevationMDataElement();
 513         case ElevationFt:
 514             return getElevationFtDataElement();
 515         case HeadingDeg:
 516             return getHeadingDegDataElement();
 517         case HeadingRad:
 518             return getHeadingRadDataElement();
 519         case PitchDeg:
 520             return getPitchDegDataElement();
 521         case PitchRad:
 522             return getPitchRadDataElement();
 523         case RollDeg:
 524             return getRollDegDataElement();
 525         case RollRad:
 526             return getRollRadDataElement();
 527         case GroundTrackDeg:
 528             return getGroundTrackDegDataElement();
 529         case GroundTrackRad:
 530             return getGroundTrackRadDataElement();
 531         case GroundSpeedKnots:
 532             return getGroundSpeedKnotsDataElement();
 533         case GroundSpeedFtPerSec:
 534             return getGroundSpeedFtPerSecDataElement();
 535         case GroundSpeedMPerSec:
 536             return getGroundSpeedMPerSecDataElement();
 537         case VerticalSpeedFtPerSec:
 538             return getVerticalSpeedFtPerSecDataElement();
 539         case VerticalSpeedFtPerMin:
 540             return getVerticalSpeedFtPerMinDataElement();
 541         case VerticalSpeedMPerSec:
 542             return getVerticalSpeedMPerSecDataElement();
 543         default:
 544             return 0;
 545         }
 546     }
 547
 548 private:
 549     template<double (HLAGeodeticLocation::*getter)() const,
 550              void (HLAGeodeticLocation::*setter)(double)>
 551     class DataElement : public HLAAbstractDoubleDataElement {
 552     public:
 553         DataElement(HLAGeodeticLocation* data) :
 554             _data(data)
 555         { }
 556         virtual double getValue() const
 557         { return (_data->*getter)(); }
 558         virtual void setValue(double value)
 559         { (_data->*setter)(value); }
 560
 561     private:
 562         SGSharedPtr<HLAGeodeticLocation> _data;
 563     };
 564
 565     void updateGeodeticFromCartesian() const
 566     {
 567         if (!_dirty)
 568             return;
 569         _geodPosition = SGGeod::fromCart(_cartPosition);
 570         SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
 571         geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
 572         SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
 573         if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
 574             _groundTrackRad = atan2(nedVel[1], nedVel[0]);
 575         else
 576             _groundTrackRad = 0;
 577         _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
 578         _verticalSpeedMPerSec = -nedVel[2];
 579         _dirty = false;
 580     }
 581     void updateCartesianFromGeodetic() const
 582     {
 583         if (!_dirty)
 584             return;
 585         _cartPosition = SGVec3d::fromGeod(_geodPosition);
 586         SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
 587         _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
 588         SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
 589                        sin(_groundTrackRad)*_groundSpeedMPerSec,
 590                        -_verticalSpeedMPerSec);
 591         _cartBodyVelocity = geodOrientation.transform(nedVel);
 592         _dirty = false;
 593     }
 594
 595     mutable bool _dirty;
 596
 597     // the cartesian values
 598     mutable SGVec3d _cartPosition;
 599     mutable SGQuatd _cartOrientation;
 600     mutable SGVec3d _cartBodyVelocity;
 601
 602     // The geodetic values
 603     mutable SGGeod _geodPosition;
 604     mutable SGVec3d _geodEulerRad;
 605     mutable double _groundTrackRad;
 606     mutable double _groundSpeedMPerSec;
 607     mutable double _verticalSpeedMPerSec;
 608 };
 609
 610 class HLAGeodeticLocationFactory : public HLALocationFactory {
 611 public:
 612     enum Semantic {
 613         LatitudeDeg = HLAGeodeticLocation::LatitudeDeg,
 614         LatitudeRad = HLAGeodeticLocation::LatitudeRad,
 615         LongitudeDeg = HLAGeodeticLocation::LongitudeDeg,
 616         LongitudeRad = HLAGeodeticLocation::LongitudeRad,
 617         ElevationM = HLAGeodeticLocation::ElevationM,
 618         ElevationFt = HLAGeodeticLocation::ElevationFt,
 619         HeadingDeg = HLAGeodeticLocation::HeadingDeg,
 620         HeadingRad = HLAGeodeticLocation::HeadingRad,
 621         PitchDeg = HLAGeodeticLocation::PitchDeg,
 622         PitchRad = HLAGeodeticLocation::PitchRad,
 623         RollDeg = HLAGeodeticLocation::RollDeg,
 624         RollRad = HLAGeodeticLocation::RollRad,
 625         GroundTrackDeg = HLAGeodeticLocation::GroundTrackDeg,
 626         GroundTrackRad = HLAGeodeticLocation::GroundTrackRad,
 627         GroundSpeedKnots = HLAGeodeticLocation::GroundSpeedKnots,
 628         GroundSpeedFtPerSec = HLAGeodeticLocation::GroundSpeedFtPerSec,
 629         GroundSpeedMPerSec = HLAGeodeticLocation::GroundSpeedMPerSec,
 630         VerticalSpeedFtPerSec = HLAGeodeticLocation::VerticalSpeedFtPerSec,
 631         VerticalSpeedFtPerMin = HLAGeodeticLocation::VerticalSpeedFtPerMin,
 632         VerticalSpeedMPerSec = HLAGeodeticLocation::VerticalSpeedMPerSec
 633     };
 634
 635     virtual HLAGeodeticLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
 636     {
 637         HLAGeodeticLocation* location = new HLAGeodeticLocation;
 638
 639         for (IndexPathPairSemanticMap::const_iterator i = _indexPathPairSemanticMap.begin();
 640              i != _indexPathPairSemanticMap.end(); ++i) {
 641             HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
 642             attributePathElementMap[i->first.first][i->first.second] = location->getDataElement(semantic);
 643         }
 644
 645         return location;
 646     }
 647
 648     virtual HLAGeodeticLocation* createLocation(HLAObjectInstance& objectInstance) const
 649     {
 650         HLAGeodeticLocation* location = new HLAGeodeticLocation;
 651
 652         for (IndexPathPairSemanticMap::const_iterator i = _indexPathPairSemanticMap.begin();
 653              i != _indexPathPairSemanticMap.end(); ++i) {
 654             std::string path = objectInstance.getAttributeName(i->first.first);
 655             if (!i->first.second.empty())
 656                 path += HLADataElement::toString(i->first.second);
 657             HLADataElementIndex index;
 658             if (!objectInstance.getDataElementIndex(index, path))
 659                 continue;
 660             HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
 661             objectInstance.setAttributeDataElement(index, location->getDataElement(semantic));
 662         }
 663
 664         for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
 665              i != _indexSemanticMap.end(); ++i) {
 666             HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
 667             objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
 668         }
 669
 670         return location;
 671     }
 672
 673     void setIndexPathPair(Semantic semantic, const HLADataElement::IndexPathPair& indexPathPair)
 674     { _indexPathPairSemanticMap[indexPathPair] = semantic; }
 675     void setIndex(Semantic semantic, const HLADataElementIndex& index)
 676     { _indexSemanticMap[index] = semantic; }
 677
 678 private:
 679     typedef std::map<HLADataElement::IndexPathPair, Semantic> IndexPathPairSemanticMap;
 680     IndexPathPairSemanticMap _indexPathPairSemanticMap;
 681
 682     typedef std::map<HLADataElementIndex, Semantic> IndexSemanticMap;
 683     IndexSemanticMap _indexSemanticMap;
 684 };
 685
 686 } // namespace simgear
 687
 688 #endif