simgear/hla/HLALocation.cxx

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