hla: Use an implementation file for SGLocation.
This commit is contained in:
Mathias Froehlich
@@ -41,6 +41,7 @@ set(HLA_SOURCES
|
||||
HLAInteractionClass.cxx
|
||||
HLAFixedRecordDataElement.cxx
|
||||
HLAFixedRecordDataType.cxx
|
||||
HLALocation.cxx
|
||||
HLAObjectClass.cxx
|
||||
HLAObjectInstance.cxx
|
||||
HLAOMTXmlVisitor.cxx
|
||||
|
||||
578
simgear/hla/HLALocation.cxx
Normal file
578
simgear/hla/HLALocation.cxx
Normal file
@@ -0,0 +1,578 @@
|
||||
// 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.
|
||||
//
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
# include <simgear_config.h>
|
||||
#endif
|
||||
|
||||
#include <simgear/compiler.h>
|
||||
|
||||
#include "HLALocation.hxx"
|
||||
|
||||
#include "HLAArrayDataElement.hxx"
|
||||
#include "HLAObjectInstance.hxx"
|
||||
|
||||
namespace simgear {
|
||||
|
||||
HLAAbstractLocationFactory::~HLAAbstractLocationFactory()
|
||||
{
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
HLACartesianLocation::HLACartesianLocation()
|
||||
{
|
||||
}
|
||||
|
||||
HLACartesianLocation::~HLACartesianLocation()
|
||||
{
|
||||
}
|
||||
|
||||
SGLocationd
|
||||
HLACartesianLocation::getLocation() const
|
||||
{
|
||||
return SGLocationd(getCartPosition(), getCartOrientation());
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocation::setLocation(const SGLocationd& location)
|
||||
{
|
||||
setCartPosition(location.getPosition());
|
||||
setCartOrientation(location.getOrientation());
|
||||
}
|
||||
|
||||
SGVec3d
|
||||
HLACartesianLocation::getCartPosition() const
|
||||
{
|
||||
return SGVec3d(_position[0].getValue(), _position[1].getValue(), _position[2].getValue());
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocation::setCartPosition(const SGVec3d& position)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
_position[i].setValue(position[i]);
|
||||
}
|
||||
|
||||
SGQuatd
|
||||
HLACartesianLocation::getCartOrientation() const
|
||||
{
|
||||
return SGQuatd::fromPositiveRealImag(SGVec3d(_imag[0].getValue(), _imag[1].getValue(), _imag[2].getValue()));
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocation::setCartOrientation(const SGQuatd& orientation)
|
||||
{
|
||||
SGVec3d imag = orientation.getPositiveRealImag();
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
_imag[i].setValue(imag[i]);
|
||||
}
|
||||
|
||||
SGVec3d
|
||||
HLACartesianLocation::getAngularBodyVelocity() const
|
||||
{
|
||||
return SGVec3d(_angularVelocity[0].getValue(), _angularVelocity[1].getValue(), _angularVelocity[2].getValue());
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocation::setAngularBodyVelocity(const SGVec3d& angularVelocity)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
_angularVelocity[i].setValue(angularVelocity[i]);
|
||||
}
|
||||
|
||||
SGVec3d
|
||||
HLACartesianLocation::getLinearBodyVelocity() const
|
||||
{
|
||||
return SGVec3d(_linearVelocity[0].getValue(), _linearVelocity[1].getValue(), _linearVelocity[2].getValue());
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocation::setLinearBodyVelocity(const SGVec3d& linearVelocity)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
_linearVelocity[i].setValue(linearVelocity[i]);
|
||||
}
|
||||
|
||||
double
|
||||
HLACartesianLocation::getTimeDifference(const SGTimeStamp& timeStamp) const
|
||||
{
|
||||
return _position[0].getDataElement()->getTimeDifference(timeStamp);
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getPositionDataElement()
|
||||
{
|
||||
HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
arrayDataElement->setElement(i, _position[i].getDataElement());
|
||||
return arrayDataElement;
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getPositionDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return _position[i].getDataElement();
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getOrientationDataElement()
|
||||
{
|
||||
HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
arrayDataElement->setElement(i, _imag[i].getDataElement());
|
||||
return arrayDataElement;
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getOrientationDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return _imag[i].getDataElement();
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getAngularVelocityDataElement()
|
||||
{
|
||||
HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
arrayDataElement->setElement(i, _angularVelocity[i].getDataElement());
|
||||
return arrayDataElement;
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getAngularVelocityDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return _angularVelocity[i].getDataElement();
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getLinearVelocityDataElement()
|
||||
{
|
||||
HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
arrayDataElement->setElement(i, _linearVelocity[i].getDataElement());
|
||||
return arrayDataElement;
|
||||
}
|
||||
|
||||
HLADataElement*
|
||||
HLACartesianLocation::getLinearVelocityDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return _linearVelocity[i].getDataElement();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
HLACartesianLocationFactory::HLACartesianLocationFactory()
|
||||
{
|
||||
}
|
||||
|
||||
HLACartesianLocationFactory::~HLACartesianLocationFactory()
|
||||
{
|
||||
}
|
||||
|
||||
HLACartesianLocation*
|
||||
HLACartesianLocationFactory::createLocation(HLAObjectInstance& objectInstance) const
|
||||
{
|
||||
HLACartesianLocation* location = new HLACartesianLocation;
|
||||
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_positionIndex[i], location->getPositionDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
|
||||
|
||||
return location;
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setPositionIndex(const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
_positionIndex[i] = dataElementIndex;
|
||||
_positionIndex[i].push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_positionIndex[index] = dataElementIndex;
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setOrientationIndex(const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
_orientationIndex[i] = dataElementIndex;
|
||||
_orientationIndex[i].push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_orientationIndex[index] = dataElementIndex;
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setAngularVelocityIndex(const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
_angularVelocityIndex[i] = dataElementIndex;
|
||||
_angularVelocityIndex[i].push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_angularVelocityIndex[index] = dataElementIndex;
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setLinearVelocityIndex(const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
_linearVelocityIndex[i] = dataElementIndex;
|
||||
_linearVelocityIndex[i].push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
HLACartesianLocationFactory::setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_linearVelocityIndex[index] = dataElementIndex;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class HLAGeodeticLocationFactory::Location : public HLAAbstractLocation {
|
||||
public:
|
||||
Location() :
|
||||
_dirty(true),
|
||||
_cartPosition(SGVec3d::zeros()),
|
||||
_cartOrientation(SGQuatd::unit()),
|
||||
_cartBodyVelocity(SGVec3d::zeros()),
|
||||
_geodPosition(),
|
||||
_geodEulerRad(SGVec3d::zeros()),
|
||||
_groundTrackRad(0),
|
||||
_groundSpeedMPerSec(0),
|
||||
_verticalSpeedMPerSec(0)
|
||||
{
|
||||
updateCartesianFromGeodetic();
|
||||
}
|
||||
virtual ~Location()
|
||||
{
|
||||
}
|
||||
|
||||
virtual SGLocationd getLocation() const
|
||||
{
|
||||
updateCartesianFromGeodetic();
|
||||
return SGLocationd(_cartPosition, _cartOrientation);
|
||||
}
|
||||
virtual void setLocation(const SGLocationd& location)
|
||||
{
|
||||
_cartPosition = location.getPosition();
|
||||
_cartOrientation = location.getOrientation();
|
||||
_dirty = true;
|
||||
}
|
||||
|
||||
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) \
|
||||
HLADataElement* get## name ## DataElement() \
|
||||
{ return new DataElement<&Location::get## name, &Location::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
|
||||
|
||||
HLADataElement* getDataElement(Semantic semantic)
|
||||
{
|
||||
switch (semantic) {
|
||||
case LatitudeDeg:
|
||||
return getLatitudeDegDataElement();
|
||||
case LatitudeRad:
|
||||
return getLatitudeRadDataElement();
|
||||
case LongitudeDeg:
|
||||
return getLongitudeDegDataElement();
|
||||
case LongitudeRad:
|
||||
return getLongitudeRadDataElement();
|
||||
case ElevationM:
|
||||
return getElevationMDataElement();
|
||||
case ElevationFt:
|
||||
return getElevationFtDataElement();
|
||||
case HeadingDeg:
|
||||
return getHeadingDegDataElement();
|
||||
case HeadingRad:
|
||||
return getHeadingRadDataElement();
|
||||
case PitchDeg:
|
||||
return getPitchDegDataElement();
|
||||
case PitchRad:
|
||||
return getPitchRadDataElement();
|
||||
case RollDeg:
|
||||
return getRollDegDataElement();
|
||||
case RollRad:
|
||||
return getRollRadDataElement();
|
||||
case GroundTrackDeg:
|
||||
return getGroundTrackDegDataElement();
|
||||
case GroundTrackRad:
|
||||
return getGroundTrackRadDataElement();
|
||||
case GroundSpeedKnots:
|
||||
return getGroundSpeedKnotsDataElement();
|
||||
case GroundSpeedFtPerSec:
|
||||
return getGroundSpeedFtPerSecDataElement();
|
||||
case GroundSpeedMPerSec:
|
||||
return getGroundSpeedMPerSecDataElement();
|
||||
case VerticalSpeedFtPerSec:
|
||||
return getVerticalSpeedFtPerSecDataElement();
|
||||
case VerticalSpeedFtPerMin:
|
||||
return getVerticalSpeedFtPerMinDataElement();
|
||||
case VerticalSpeedMPerSec:
|
||||
return getVerticalSpeedMPerSecDataElement();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
template<double (Location::*getter)() const, void (Location::*setter)(double)>
|
||||
class DataElement : public HLAAbstractDoubleDataElement {
|
||||
public:
|
||||
DataElement(Location* data) :
|
||||
_data(data)
|
||||
{ }
|
||||
virtual double getValue() const
|
||||
{ return (_data->*getter)(); }
|
||||
virtual void setValue(double value)
|
||||
{ (_data->*setter)(value); }
|
||||
|
||||
private:
|
||||
SGSharedPtr<Location> _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;
|
||||
};
|
||||
|
||||
HLAGeodeticLocationFactory::HLAGeodeticLocationFactory()
|
||||
{
|
||||
}
|
||||
|
||||
HLAGeodeticLocationFactory::~HLAGeodeticLocationFactory()
|
||||
{
|
||||
}
|
||||
|
||||
HLAAbstractLocation*
|
||||
HLAGeodeticLocationFactory::createLocation(HLAObjectInstance& objectInstance) const
|
||||
{
|
||||
Location* location = new Location;
|
||||
|
||||
for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
|
||||
i != _indexSemanticMap.end(); ++i) {
|
||||
Semantic semantic = i->second;
|
||||
objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
|
||||
}
|
||||
|
||||
return location;
|
||||
}
|
||||
|
||||
} // namespace simgear
|
||||
@@ -24,6 +24,8 @@
|
||||
|
||||
namespace simgear {
|
||||
|
||||
class HLAObjectInstance;
|
||||
|
||||
class HLAAbstractLocation : public SGReferenced {
|
||||
public:
|
||||
virtual ~HLAAbstractLocation() {}
|
||||
@@ -57,194 +59,77 @@ public:
|
||||
|
||||
class HLACartesianLocation : public HLAAbstractLocation {
|
||||
public:
|
||||
HLACartesianLocation() :
|
||||
_position(SGVec3d::zeros()),
|
||||
_imag(SGVec3d::zeros()),
|
||||
_angularVelocity(SGVec3d::zeros()),
|
||||
_linearVelocity(SGVec3d::zeros())
|
||||
{ }
|
||||
HLACartesianLocation();
|
||||
virtual ~HLACartesianLocation();
|
||||
|
||||
virtual SGLocationd getLocation() const
|
||||
{ return SGLocationd(_position, SGQuatd::fromPositiveRealImag(_imag)); }
|
||||
virtual void setLocation(const SGLocationd& location)
|
||||
{
|
||||
_position = location.getPosition();
|
||||
_imag = location.getOrientation().getPositiveRealImag();
|
||||
}
|
||||
virtual SGLocationd getLocation() const;
|
||||
virtual void setLocation(const SGLocationd& location);
|
||||
|
||||
virtual SGVec3d getCartPosition() const
|
||||
{ return _position; }
|
||||
virtual void setCartPosition(const SGVec3d& position)
|
||||
{ _position = position; }
|
||||
virtual SGVec3d getCartPosition() const;
|
||||
virtual void setCartPosition(const SGVec3d& position);
|
||||
|
||||
virtual SGQuatd getCartOrientation() const
|
||||
{ return SGQuatd::fromPositiveRealImag(_imag); }
|
||||
virtual void setCartOrientation(const SGQuatd& orientation)
|
||||
{ _imag = orientation.getPositiveRealImag(); }
|
||||
virtual SGQuatd getCartOrientation() const;
|
||||
virtual void setCartOrientation(const SGQuatd& orientation);
|
||||
|
||||
virtual SGVec3d getAngularBodyVelocity() const
|
||||
{ return _angularVelocity; }
|
||||
virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
|
||||
{ _angularVelocity = angularVelocity; }
|
||||
virtual SGVec3d getAngularBodyVelocity() const;
|
||||
virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity);
|
||||
|
||||
virtual SGVec3d getLinearBodyVelocity() const
|
||||
{ return _linearVelocity; }
|
||||
virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
|
||||
{ _linearVelocity = linearVelocity; }
|
||||
virtual SGVec3d getLinearBodyVelocity() const;
|
||||
virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity);
|
||||
|
||||
HLADataElement* getPositionDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return new PositionDataElement(this, i);
|
||||
}
|
||||
HLADataElement* getOrientationDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return new OrientationDataElement(this, i);
|
||||
}
|
||||
virtual double getTimeDifference(const SGTimeStamp& timeStamp) const;
|
||||
|
||||
HLADataElement* getAngularVelocityDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return new AngularVelocityDataElement(this, i);
|
||||
}
|
||||
HLADataElement* getLinearVelocityDataElement(unsigned i)
|
||||
{
|
||||
if (3 <= i)
|
||||
return 0;
|
||||
return new LinearVelocityDataElement(this, i);
|
||||
}
|
||||
HLADataElement* getPositionDataElement();
|
||||
HLADataElement* getPositionDataElement(unsigned i);
|
||||
HLADataElement* getOrientationDataElement();
|
||||
HLADataElement* getOrientationDataElement(unsigned i);
|
||||
HLADataElement* getAngularVelocityDataElement();
|
||||
HLADataElement* getAngularVelocityDataElement(unsigned i);
|
||||
HLADataElement* getLinearVelocityDataElement();
|
||||
HLADataElement* getLinearVelocityDataElement(unsigned 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; }
|
||||
HLADoubleData _position[3];
|
||||
HLADoubleData _imag[3];
|
||||
|
||||
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;
|
||||
HLADoubleData _angularVelocity[3];
|
||||
HLADoubleData _linearVelocity[3];
|
||||
};
|
||||
|
||||
class HLALocationFactory : public SGReferenced {
|
||||
class HLAAbstractLocationFactory : public SGReferenced {
|
||||
public:
|
||||
virtual ~HLALocationFactory() {}
|
||||
virtual ~HLAAbstractLocationFactory();
|
||||
virtual HLAAbstractLocation* createLocation(HLAObjectInstance&) const = 0;
|
||||
};
|
||||
|
||||
class HLACartesianLocationFactory : public HLALocationFactory {
|
||||
typedef HLAAbstractLocationFactory HLALocationFactory;
|
||||
|
||||
class HLACartesianLocationFactory : public HLAAbstractLocationFactory {
|
||||
public:
|
||||
virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const
|
||||
{
|
||||
HLACartesianLocation* location = new HLACartesianLocation;
|
||||
HLACartesianLocationFactory();
|
||||
virtual ~HLACartesianLocationFactory();
|
||||
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_positonIndex[i], location->getPositionDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
|
||||
for (unsigned i = 0; i < 3; ++i)
|
||||
objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
|
||||
virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const;
|
||||
|
||||
return location;
|
||||
}
|
||||
void setPositionIndex(const HLADataElementIndex& dataElementIndex);
|
||||
void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex);
|
||||
void setOrientationIndex(const HLADataElementIndex& dataElementIndex);
|
||||
void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex);
|
||||
|
||||
void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_positonIndex[index] = dataElementIndex;
|
||||
}
|
||||
void setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_orientationIndex[index] = dataElementIndex;
|
||||
}
|
||||
|
||||
void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_angularVelocityIndex[index] = dataElementIndex;
|
||||
}
|
||||
void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
|
||||
{
|
||||
if (3 <= index)
|
||||
return;
|
||||
_linearVelocityIndex[index] = dataElementIndex;
|
||||
}
|
||||
void setAngularVelocityIndex(const HLADataElementIndex& dataElementIndex);
|
||||
void setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex);
|
||||
void setLinearVelocityIndex(const HLADataElementIndex& dataElementIndex);
|
||||
void setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex);
|
||||
|
||||
private:
|
||||
HLADataElementIndex _positonIndex[3];
|
||||
HLADataElementIndex _positionIndex[3];
|
||||
HLADataElementIndex _orientationIndex[3];
|
||||
|
||||
HLADataElementIndex _angularVelocityIndex[3];
|
||||
HLADataElementIndex _linearVelocityIndex[3];
|
||||
};
|
||||
|
||||
class HLAGeodeticLocation : public HLAAbstractLocation {
|
||||
class HLAGeodeticLocationFactory : public HLAAbstractLocationFactory {
|
||||
public:
|
||||
enum Semantic {
|
||||
LatitudeDeg,
|
||||
@@ -269,318 +154,17 @@ public:
|
||||
VerticalSpeedMPerSec
|
||||
};
|
||||
|
||||
HLAGeodeticLocation() :
|
||||
_dirty(true),
|
||||
_cartPosition(SGVec3d::zeros()),
|
||||
_cartOrientation(SGQuatd::unit()),
|
||||
_cartBodyVelocity(SGVec3d::zeros()),
|
||||
_geodPosition(),
|
||||
_geodEulerRad(SGVec3d::zeros()),
|
||||
_groundTrackRad(0),
|
||||
_groundSpeedMPerSec(0),
|
||||
_verticalSpeedMPerSec(0)
|
||||
{
|
||||
updateCartesianFromGeodetic();
|
||||
}
|
||||
HLAGeodeticLocationFactory();
|
||||
virtual ~HLAGeodeticLocationFactory();
|
||||
|
||||
virtual SGLocationd getLocation() const
|
||||
{
|
||||
updateCartesianFromGeodetic();
|
||||
return SGLocationd(_cartPosition, _cartOrientation);
|
||||
}
|
||||
virtual void setLocation(const SGLocationd& location)
|
||||
{
|
||||
_cartPosition = location.getPosition();
|
||||
_cartOrientation = location.getOrientation();
|
||||
_dirty = true;
|
||||
}
|
||||
|
||||
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) \
|
||||
HLADataElement* 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
|
||||
|
||||
HLADataElement* getDataElement(Semantic semantic)
|
||||
{
|
||||
switch (semantic) {
|
||||
case LatitudeDeg:
|
||||
return getLatitudeDegDataElement();
|
||||
case LatitudeRad:
|
||||
return getLatitudeRadDataElement();
|
||||
case LongitudeDeg:
|
||||
return getLongitudeDegDataElement();
|
||||
case LongitudeRad:
|
||||
return getLongitudeRadDataElement();
|
||||
case ElevationM:
|
||||
return getElevationMDataElement();
|
||||
case ElevationFt:
|
||||
return getElevationFtDataElement();
|
||||
case HeadingDeg:
|
||||
return getHeadingDegDataElement();
|
||||
case HeadingRad:
|
||||
return getHeadingRadDataElement();
|
||||
case PitchDeg:
|
||||
return getPitchDegDataElement();
|
||||
case PitchRad:
|
||||
return getPitchRadDataElement();
|
||||
case RollDeg:
|
||||
return getRollDegDataElement();
|
||||
case RollRad:
|
||||
return getRollRadDataElement();
|
||||
case GroundTrackDeg:
|
||||
return getGroundTrackDegDataElement();
|
||||
case GroundTrackRad:
|
||||
return getGroundTrackRadDataElement();
|
||||
case GroundSpeedKnots:
|
||||
return getGroundSpeedKnotsDataElement();
|
||||
case GroundSpeedFtPerSec:
|
||||
return getGroundSpeedFtPerSecDataElement();
|
||||
case GroundSpeedMPerSec:
|
||||
return getGroundSpeedMPerSecDataElement();
|
||||
case VerticalSpeedFtPerSec:
|
||||
return getVerticalSpeedFtPerSecDataElement();
|
||||
case VerticalSpeedFtPerMin:
|
||||
return getVerticalSpeedFtPerMinDataElement();
|
||||
case VerticalSpeedMPerSec:
|
||||
return getVerticalSpeedMPerSecDataElement();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
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 = HLAGeodeticLocation::LatitudeDeg,
|
||||
LatitudeRad = HLAGeodeticLocation::LatitudeRad,
|
||||
LongitudeDeg = HLAGeodeticLocation::LongitudeDeg,
|
||||
LongitudeRad = HLAGeodeticLocation::LongitudeRad,
|
||||
ElevationM = HLAGeodeticLocation::ElevationM,
|
||||
ElevationFt = HLAGeodeticLocation::ElevationFt,
|
||||
HeadingDeg = HLAGeodeticLocation::HeadingDeg,
|
||||
HeadingRad = HLAGeodeticLocation::HeadingRad,
|
||||
PitchDeg = HLAGeodeticLocation::PitchDeg,
|
||||
PitchRad = HLAGeodeticLocation::PitchRad,
|
||||
RollDeg = HLAGeodeticLocation::RollDeg,
|
||||
RollRad = HLAGeodeticLocation::RollRad,
|
||||
GroundTrackDeg = HLAGeodeticLocation::GroundTrackDeg,
|
||||
GroundTrackRad = HLAGeodeticLocation::GroundTrackRad,
|
||||
GroundSpeedKnots = HLAGeodeticLocation::GroundSpeedKnots,
|
||||
GroundSpeedFtPerSec = HLAGeodeticLocation::GroundSpeedFtPerSec,
|
||||
GroundSpeedMPerSec = HLAGeodeticLocation::GroundSpeedMPerSec,
|
||||
VerticalSpeedFtPerSec = HLAGeodeticLocation::VerticalSpeedFtPerSec,
|
||||
VerticalSpeedFtPerMin = HLAGeodeticLocation::VerticalSpeedFtPerMin,
|
||||
VerticalSpeedMPerSec = HLAGeodeticLocation::VerticalSpeedMPerSec
|
||||
};
|
||||
|
||||
virtual HLAGeodeticLocation* createLocation(HLAObjectInstance& objectInstance) const
|
||||
{
|
||||
HLAGeodeticLocation* location = new HLAGeodeticLocation;
|
||||
|
||||
for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
|
||||
i != _indexSemanticMap.end(); ++i) {
|
||||
HLAGeodeticLocation::Semantic semantic = HLAGeodeticLocation::Semantic(i->second);
|
||||
objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
|
||||
}
|
||||
|
||||
return location;
|
||||
}
|
||||
virtual HLAAbstractLocation* createLocation(HLAObjectInstance& objectInstance) const;
|
||||
|
||||
void setIndex(Semantic semantic, const HLADataElementIndex& index)
|
||||
{ _indexSemanticMap[index] = semantic; }
|
||||
|
||||
private:
|
||||
class Location;
|
||||
|
||||
typedef std::map<HLADataElementIndex, Semantic> IndexSemanticMap;
|
||||
IndexSemanticMap _indexSemanticMap;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user