X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fhla%2FHLALocation.hxx;h=d19ec8bb10e29006fa9b46ea43d5c13b797a364b;hb=16ef5a0a49330adfb4eec8caa56cf37bde345123;hp=a43f516062369d21accec5fea3a541270871d5b4;hpb=d4310a7f3b27b8403ba1ec030cfd3b4c6c565f24;p=simgear.git diff --git a/simgear/hla/HLALocation.hxx b/simgear/hla/HLALocation.hxx index a43f5160..d19ec8bb 100644 --- a/simgear/hla/HLALocation.hxx +++ b/simgear/hla/HLALocation.hxx @@ -24,6 +24,8 @@ namespace simgear { +class HLAObjectInstance; + class HLAAbstractLocation : public SGReferenced { public: virtual ~HLAAbstractLocation() {} @@ -50,201 +52,84 @@ public: SGLocationd getLocation(const SGTimeStamp& timeStamp) const { SGLocationd location = getLocation(); - location.eulerStepBodyVelocities(getTimeDifference(timeStamp), getLinearBodyVelocity(), getAngularBodyVelocity()); + location.eulerStepBodyVelocitiesMidOrientation(getTimeDifference(timeStamp), getLinearBodyVelocity(), getAngularBodyVelocity()); return location; } }; class HLACartesianLocation : public HLAAbstractLocation { public: - HLACartesianLocation() : - _position(SGVec3d::zeros()), - _imag(SGVec3d::zeros()), - _angularVelocity(SGVec3d::zeros()), - _linearVelocity(SGVec3d::zeros()) - { } - - virtual SGLocationd getLocation() const - { return SGLocationd(_position, SGQuatd::fromPositiveRealImag(_imag)); } - virtual void setLocation(const SGLocationd& location) - { - _position = location.getPosition(); - _imag = location.getOrientation().getPositiveRealImag(); - } + HLACartesianLocation(); + virtual ~HLACartesianLocation(); - virtual SGVec3d getCartPosition() const - { return _position; } - virtual void setCartPosition(const SGVec3d& position) - { _position = position; } + virtual SGLocationd getLocation() const; + virtual void setLocation(const SGLocationd& location); - virtual SGQuatd getCartOrientation() const - { return SGQuatd::fromPositiveRealImag(_imag); } - virtual void setCartOrientation(const SGQuatd& orientation) - { _imag = orientation.getPositiveRealImag(); } + virtual SGVec3d getCartPosition() const; + virtual void setCartPosition(const SGVec3d& position); - virtual SGVec3d getAngularBodyVelocity() const - { return _angularVelocity; } - virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity) - { _angularVelocity = angularVelocity; } + virtual SGQuatd getCartOrientation() const; + virtual void setCartOrientation(const SGQuatd& orientation); - virtual SGVec3d getLinearBodyVelocity() const - { return _linearVelocity; } - virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity) - { _linearVelocity = linearVelocity; } + virtual SGVec3d getAngularBodyVelocity() const; + virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity); - 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 SGVec3d getLinearBodyVelocity() const; + virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity); - 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); - } - -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 _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 _data; - unsigned _index; - }; + virtual double getTimeDifference(const SGTimeStamp& timeStamp) const; - 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 _data; - unsigned _index; - }; + HLADataElement* getPositionDataElement(); + HLADataElement* getPositionDataElement(unsigned i); + HLADataElement* getOrientationDataElement(); + HLADataElement* getOrientationDataElement(unsigned i); + HLADataElement* getAngularVelocityDataElement(); + HLADataElement* getAngularVelocityDataElement(unsigned i); + HLADataElement* getLinearVelocityDataElement(); + HLADataElement* getLinearVelocityDataElement(unsigned i); - 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 _data; - unsigned _index; - }; - - SGVec3d _position; - SGVec3d _imag; +private: + HLADoubleData _position[3]; + HLADoubleData _imag[3]; - 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 { -public: - virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const - { - HLACartesianLocation* location = new HLACartesianLocation; +typedef HLAAbstractLocationFactory HLALocationFactory; - 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)); +class HLACartesianLocationFactory : public HLAAbstractLocationFactory { +public: + HLACartesianLocationFactory(); + virtual ~HLACartesianLocationFactory(); - return location; - } + virtual HLACartesianLocation* createLocation(HLAObjectInstance& objectInstance) const; - 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 setPositionIndex(const HLADataElementIndex& dataElementIndex); + void setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex); + void setOrientationIndex(const HLADataElementIndex& dataElementIndex); + void setOrientationIndex(unsigned index, const HLADataElementIndex& 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(); - } - - 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 - 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 _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; + HLAGeodeticLocationFactory(); + virtual ~HLAGeodeticLocationFactory(); - 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 IndexSemanticMap; IndexSemanticMap _indexSemanticMap; };