Files
simgear/simgear/hla/HLALocation.hxx
T

550 lines
21 KiB
C++

// Copyright (C) 2009 - 2010 Mathias Froehlich - Mathias.Froehlich@web.de
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Library General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Library General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
#ifndef HLALocation_hxx
#define HLALocation_hxx
#include "HLABasicDataElement.hxx"
namespace simgear {
class HLAAbstractLocation : public SGReferenced {
public:
virtual ~HLAAbstractLocation() {}
virtual SGVec3d getCartPosition() const = 0;
virtual void setCartPosition(const SGVec3d&) = 0;
virtual SGQuatd getCartOrientation() const = 0;
virtual void setCartOrientation(const SGQuatd&) = 0;
virtual SGVec3d getAngularBodyVelocity() const = 0;
virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
virtual SGVec3d getLinearBodyVelocity() const = 0;
virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
};
class HLACartesianLocation : public HLAAbstractLocation {
public:
HLACartesianLocation() :
_position(SGVec3d::zeros()),
_imag(SGVec3d::zeros()),
_angularVelocity(SGVec3d::zeros()),
_linearVelocity(SGVec3d::zeros())
{ }
virtual SGVec3d getCartPosition() const
{ return _position; }
virtual void setCartPosition(const SGVec3d& position)
{ _position = position; }
virtual SGQuatd getCartOrientation() const
{ return SGQuatd::fromPositiveRealImag(_imag); }
virtual void setCartOrientation(const SGQuatd& orientation)
{ _imag = orientation.getPositiveRealImag(); }
virtual SGVec3d getAngularBodyVelocity() const
{ return _angularVelocity; }
virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
{ _angularVelocity = angularVelocity; }
virtual SGVec3d getLinearBodyVelocity() const
{ return _linearVelocity; }
virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
{ _linearVelocity = linearVelocity; }
HLADataElementProvider getPositionDataElement(unsigned i)
{
if (3 <= i)
return HLADataElementProvider();
return new PositionDataElement(this, i);
}
HLADataElementProvider getOrientationDataElement(unsigned i)
{
if (3 <= i)
return HLADataElementProvider();
return new OrientationDataElement(this, i);
}
HLADataElementProvider getAngularVelocityDataElement(unsigned i)
{
if (3 <= i)
return HLADataElementProvider();
return new AngularVelocityDataElement(this, i);
}
HLADataElementProvider getLinearVelocityDataElement(unsigned i)
{
if (3 <= i)
return HLADataElementProvider();
return new LinearVelocityDataElement(this, i);
}
private:
class PositionDataElement : public HLAAbstractDoubleDataElement {
public:
PositionDataElement(HLACartesianLocation* data, unsigned index) :
_data(data), _index(index)
{ }
virtual double getValue() const
{ return _data->_position[_index]; }
virtual void setValue(double value)
{ _data->_position[_index] = value; }
private:
SGSharedPtr<HLACartesianLocation> _data;
unsigned _index;
};
class OrientationDataElement : public HLAAbstractDoubleDataElement {
public:
OrientationDataElement(HLACartesianLocation* data, unsigned index) :
_data(data), _index(index)
{ }
virtual double getValue() const
{ return _data->_imag[_index]; }
virtual void setValue(double value)
{ _data->_imag[_index] = value; }
private:
SGSharedPtr<HLACartesianLocation> _data;
unsigned _index;
};
class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
public:
AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
_data(data), _index(index)
{ }
virtual double getValue() const
{ return _data->_angularVelocity[_index]; }
virtual void setValue(double value)
{ _data->_angularVelocity[_index] = value; }
private:
SGSharedPtr<HLACartesianLocation> _data;
unsigned _index;
};
class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
public:
LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
_data(data), _index(index)
{ }
virtual double getValue() const
{ return _data->_linearVelocity[_index]; }
virtual void setValue(double value)
{ _data->_linearVelocity[_index] = value; }
private:
SGSharedPtr<HLACartesianLocation> _data;
unsigned _index;
};
SGVec3d _position;
SGVec3d _imag;
SGVec3d _angularVelocity;
SGVec3d _linearVelocity;
};
class HLALocationFactory : public SGReferenced {
public:
virtual ~HLALocationFactory() {}
virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap&) const = 0;
};
class HLACartesianLocationFactory : public HLALocationFactory {
public:
virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
{
HLACartesianLocation* location = new HLACartesianLocation;
for (unsigned i = 0; i < 3; ++i) {
const HLADataElement::IndexPathPair& indexPathPair = _positonIndexPathPair[i];
attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getPositionDataElement(i);
}
for (unsigned i = 0; i < 3; ++i) {
const HLADataElement::IndexPathPair& indexPathPair = _orientationIndexPathPair[i];
attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getOrientationDataElement(i);
}
for (unsigned i = 0; i < 3; ++i) {
const HLADataElement::IndexPathPair& indexPathPair = _angularVelocityIndexPathPair[i];
attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getAngularVelocityDataElement(i);
}
for (unsigned i = 0; i < 3; ++i) {
const HLADataElement::IndexPathPair& indexPathPair = _linearVelocityIndexPathPair[i];
attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getLinearVelocityDataElement(i);
}
return location;
}
void setPositionIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
{
if (3 <= index)
return;
_positonIndexPathPair[index] = indexPathPair;
}
void setOrientationIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
{
if (3 <= index)
return;
_orientationIndexPathPair[index] = indexPathPair;
}
void setAngularVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
{
if (3 <= index)
return;
_angularVelocityIndexPathPair[index] = indexPathPair;
}
void setLinearVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
{
if (3 <= index)
return;
_linearVelocityIndexPathPair[index] = indexPathPair;
}
private:
HLADataElement::IndexPathPair _positonIndexPathPair[3];
HLADataElement::IndexPathPair _orientationIndexPathPair[3];
HLADataElement::IndexPathPair _angularVelocityIndexPathPair[3];
HLADataElement::IndexPathPair _linearVelocityIndexPathPair[3];
};
class HLAGeodeticLocation : public HLAAbstractLocation {
public:
HLAGeodeticLocation() :
_dirty(true),
_cartPosition(SGVec3d::zeros()),
_cartOrientation(SGQuatd::unit()),
_cartBodyVelocity(SGVec3d::zeros()),
_geodPosition(),
_geodEulerRad(SGVec3d::zeros()),
_groundTrackRad(0),
_groundSpeedMPerSec(0),
_verticalSpeedMPerSec(0)
{
updateCartesianFromGeodetic();
}
virtual SGVec3d getCartPosition() const
{ updateCartesianFromGeodetic(); return _cartPosition; }
virtual void setCartPosition(const SGVec3d& position)
{ _cartPosition = position; _dirty = true; }
virtual SGQuatd getCartOrientation() const
{ updateCartesianFromGeodetic(); return _cartOrientation; }
virtual void setCartOrientation(const SGQuatd& orientation)
{ _cartOrientation = orientation; _dirty = true; }
virtual SGVec3d getAngularBodyVelocity() const
{ return SGVec3d::zeros(); }
virtual void setAngularBodyVelocity(const SGVec3d& angular)
{ }
virtual SGVec3d getLinearBodyVelocity() const
{ updateCartesianFromGeodetic(); return _cartBodyVelocity; }
virtual void setLinearBodyVelocity(const SGVec3d& linear)
{ _cartBodyVelocity = linear; _dirty = true; }
void setLatitudeDeg(double value)
{ _geodPosition.setLatitudeDeg(value); _dirty = true; }
double getLatitudeDeg() const
{ updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
void setLatitudeRad(double value)
{ _geodPosition.setLatitudeRad(value); _dirty = true; }
double getLatitudeRad() const
{ updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
void setLongitudeDeg(double value)
{ _geodPosition.setLongitudeDeg(value); _dirty = true; }
double getLongitudeDeg() const
{ updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
void setLongitudeRad(double value)
{ _geodPosition.setLongitudeRad(value); _dirty = true; }
double getLongitudeRad() const
{ updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
void setElevationFt(double value)
{ _geodPosition.setElevationFt(value); _dirty = true; }
double getElevationFt() const
{ updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
void setElevationM(double value)
{ _geodPosition.setElevationM(value); _dirty = true; }
double getElevationM() const
{ updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
void setHeadingRad(double value)
{ _geodEulerRad[2] = value; _dirty = true; }
double getHeadingRad() const
{ updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
void setHeadingDeg(double value)
{ setHeadingRad(SGMiscd::deg2rad(value)); }
double getHeadingDeg() const
{ return SGMiscd::rad2deg(getHeadingRad()); }
void setPitchRad(double value)
{ _geodEulerRad[1] = value; _dirty = true; }
double getPitchRad() const
{ updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
void setPitchDeg(double value)
{ setPitchRad(SGMiscd::deg2rad(value)); }
double getPitchDeg() const
{ return SGMiscd::rad2deg(getPitchRad()); }
void setRollRad(double value)
{ _geodEulerRad[0] = value; _dirty = true; }
double getRollRad() const
{ updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
void setRollDeg(double value)
{ setRollRad(SGMiscd::deg2rad(value)); }
double getRollDeg() const
{ return SGMiscd::rad2deg(getRollRad()); }
void setGroundTrackRad(double value)
{ _groundTrackRad = value; _dirty = true; }
double getGroundTrackRad() const
{ updateGeodeticFromCartesian(); return _groundTrackRad; }
void setGroundTrackDeg(double value)
{ setGroundTrackRad(SGMiscd::deg2rad(value)); }
double getGroundTrackDeg() const
{ return SGMiscd::rad2deg(getGroundTrackRad()); }
void setGroundSpeedMPerSec(double value)
{ _groundSpeedMPerSec = value; _dirty = true; }
double getGroundSpeedMPerSec() const
{ updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
void setGroundSpeedFtPerSec(double value)
{ setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
double getGroundSpeedFtPerSec() const
{ return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
void setGroundSpeedKnots(double value)
{ setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
double getGroundSpeedKnots() const
{ return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
void setVerticalSpeedMPerSec(double value)
{ _verticalSpeedMPerSec = value; _dirty = true; }
double getVerticalSpeedMPerSec() const
{ updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
void setVerticalSpeedFtPerSec(double value)
{ setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
double getVerticalSpeedFtPerSec() const
{ return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
void setVerticalSpeedFtPerMin(double value)
{ setVerticalSpeedFtPerSec(value/60); }
double getVerticalSpeedFtPerMin() const
{ return 60*getVerticalSpeedFtPerSec(); }
#define DATA_ELEMENT(name) \
HLADataElementProvider get## name ## DataElement() \
{ return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
DATA_ELEMENT(LatitudeDeg)
DATA_ELEMENT(LatitudeRad)
DATA_ELEMENT(LongitudeDeg)
DATA_ELEMENT(LongitudeRad)
DATA_ELEMENT(ElevationFt)
DATA_ELEMENT(ElevationM)
DATA_ELEMENT(HeadingDeg)
DATA_ELEMENT(HeadingRad)
DATA_ELEMENT(PitchDeg)
DATA_ELEMENT(PitchRad)
DATA_ELEMENT(RollDeg)
DATA_ELEMENT(RollRad)
DATA_ELEMENT(GroundTrackDeg)
DATA_ELEMENT(GroundTrackRad)
DATA_ELEMENT(GroundSpeedMPerSec)
DATA_ELEMENT(GroundSpeedFtPerSec)
DATA_ELEMENT(GroundSpeedKnots)
DATA_ELEMENT(VerticalSpeedMPerSec)
DATA_ELEMENT(VerticalSpeedFtPerSec)
DATA_ELEMENT(VerticalSpeedFtPerMin)
#undef DATA_ELEMENT
private:
template<double (HLAGeodeticLocation::*getter)() const,
void (HLAGeodeticLocation::*setter)(double)>
class DataElement : public HLAAbstractDoubleDataElement {
public:
DataElement(HLAGeodeticLocation* data) :
_data(data)
{ }
virtual double getValue() const
{ return (_data->*getter)(); }
virtual void setValue(double value)
{ (_data->*setter)(value); }
private:
SGSharedPtr<HLAGeodeticLocation> _data;
};
void updateGeodeticFromCartesian() const
{
if (!_dirty)
return;
_geodPosition = SGGeod::fromCart(_cartPosition);
SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
_groundTrackRad = atan2(nedVel[1], nedVel[0]);
else
_groundTrackRad = 0;
_groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
_verticalSpeedMPerSec = -nedVel[2];
_dirty = false;
}
void updateCartesianFromGeodetic() const
{
if (!_dirty)
return;
_cartPosition = SGVec3d::fromGeod(_geodPosition);
SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
_cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
sin(_groundTrackRad)*_groundSpeedMPerSec,
-_verticalSpeedMPerSec);
_cartBodyVelocity = geodOrientation.transform(nedVel);
_dirty = false;
}
mutable bool _dirty;
// the cartesian values
mutable SGVec3d _cartPosition;
mutable SGQuatd _cartOrientation;
mutable SGVec3d _cartBodyVelocity;
// The geodetic values
mutable SGGeod _geodPosition;
mutable SGVec3d _geodEulerRad;
mutable double _groundTrackRad;
mutable double _groundSpeedMPerSec;
mutable double _verticalSpeedMPerSec;
};
class HLAGeodeticLocationFactory : public HLALocationFactory {
public:
enum Semantic {
LatitudeDeg,
LatitudeRad,
LongitudeDeg,
LongitudeRad,
ElevationM,
ElevationFt,
HeadingDeg,
HeadingRad,
PitchDeg,
PitchRad,
RollDeg,
RollRad,
GroundTrackDeg,
GroundTrackRad,
GroundSpeedKnots,
GroundSpeedFtPerSec,
GroundSpeedMPerSec,
VerticalSpeedFtPerSec,
VerticalSpeedFtPerMin,
VerticalSpeedMPerSec
};
virtual HLAGeodeticLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
{
HLAGeodeticLocation* location = new HLAGeodeticLocation;
for (IndexPathPairSemanticMap::const_iterator i = _indexPathPairSemanticMap.begin();
i != _indexPathPairSemanticMap.end(); ++i) {
switch (i->second) {
case LatitudeDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeDegDataElement();
break;
case LatitudeRad:
attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeRadDataElement();
break;
case LongitudeDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeDegDataElement();
break;
case LongitudeRad:
attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeRadDataElement();
break;
case ElevationM:
attributePathElementMap[i->first.first][i->first.second] = location->getElevationMDataElement();
break;
case ElevationFt:
attributePathElementMap[i->first.first][i->first.second] = location->getElevationFtDataElement();
break;
case HeadingDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getHeadingDegDataElement();
break;
case HeadingRad:
attributePathElementMap[i->first.first][i->first.second] = location->getHeadingRadDataElement();
break;
case PitchDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getPitchDegDataElement();
break;
case PitchRad:
attributePathElementMap[i->first.first][i->first.second] = location->getPitchRadDataElement();
break;
case RollDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getRollDegDataElement();
break;
case RollRad:
attributePathElementMap[i->first.first][i->first.second] = location->getRollRadDataElement();
break;
case GroundTrackDeg:
attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackDegDataElement();
break;
case GroundTrackRad:
attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackRadDataElement();
break;
case GroundSpeedKnots:
attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedKnotsDataElement();
break;
case GroundSpeedFtPerSec:
attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedFtPerSecDataElement();
break;
case GroundSpeedMPerSec:
attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedMPerSecDataElement();
break;
case VerticalSpeedFtPerSec:
attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerSecDataElement();
break;
case VerticalSpeedFtPerMin:
attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerMinDataElement();
break;
case VerticalSpeedMPerSec:
attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedMPerSecDataElement();
break;
}
}
return location;
}
void setIndexPathPair(Semantic semantic, const HLADataElement::IndexPathPair& indexPathPair)
{ _indexPathPairSemanticMap[indexPathPair] = semantic; }
private:
typedef std::map<HLADataElement::IndexPathPair, Semantic> IndexPathPairSemanticMap;
IndexPathPairSemanticMap _indexPathPairSemanticMap;
};
} // namespace simgear
#endif