1 // Copyright (C) 2009 - 2010 Mathias Froehlich - Mathias.Froehlich@web.de
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.
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.
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.
18 #ifndef HLALocation_hxx
19 #define HLALocation_hxx
21 #include "HLABasicDataElement.hxx"
25 class HLAAbstractLocation : public SGReferenced {
27 virtual ~HLAAbstractLocation() {}
29 virtual SGVec3d getCartPosition() const = 0;
30 virtual void setCartPosition(const SGVec3d&) = 0;
32 virtual SGQuatd getCartOrientation() const = 0;
33 virtual void setCartOrientation(const SGQuatd&) = 0;
35 virtual SGVec3d getAngularBodyVelocity() const = 0;
36 virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
38 virtual SGVec3d getLinearBodyVelocity() const = 0;
39 virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
42 class HLACartesianLocation : public HLAAbstractLocation {
44 HLACartesianLocation() :
45 _position(SGVec3d::zeros()),
46 _imag(SGVec3d::zeros()),
47 _angularVelocity(SGVec3d::zeros()),
48 _linearVelocity(SGVec3d::zeros())
51 virtual SGVec3d getCartPosition() const
53 virtual void setCartPosition(const SGVec3d& position)
54 { _position = position; }
56 virtual SGQuatd getCartOrientation() const
57 { return SGQuatd::fromPositiveRealImag(_imag); }
58 virtual void setCartOrientation(const SGQuatd& orientation)
59 { _imag = orientation.getPositiveRealImag(); }
61 virtual SGVec3d getAngularBodyVelocity() const
62 { return _angularVelocity; }
63 virtual void setAngularBodyVelocity(const SGVec3d& angularVelocity)
64 { _angularVelocity = angularVelocity; }
66 virtual SGVec3d getLinearBodyVelocity() const
67 { return _linearVelocity; }
68 virtual void setLinearBodyVelocity(const SGVec3d& linearVelocity)
69 { _linearVelocity = linearVelocity; }
71 HLADataElementProvider getPositionDataElement(unsigned i)
74 return HLADataElementProvider();
75 return new PositionDataElement(this, i);
77 HLADataElementProvider getOrientationDataElement(unsigned i)
80 return HLADataElementProvider();
81 return new OrientationDataElement(this, i);
84 HLADataElementProvider getAngularVelocityDataElement(unsigned i)
87 return HLADataElementProvider();
88 return new AngularVelocityDataElement(this, i);
90 HLADataElementProvider getLinearVelocityDataElement(unsigned i)
93 return HLADataElementProvider();
94 return new LinearVelocityDataElement(this, i);
98 class PositionDataElement : public HLAAbstractDoubleDataElement {
100 PositionDataElement(HLACartesianLocation* data, unsigned index) :
101 _data(data), _index(index)
103 virtual double getValue() const
104 { return _data->_position[_index]; }
105 virtual void setValue(double value)
106 { _data->_position[_index] = value; }
109 SGSharedPtr<HLACartesianLocation> _data;
113 class OrientationDataElement : public HLAAbstractDoubleDataElement {
115 OrientationDataElement(HLACartesianLocation* data, unsigned index) :
116 _data(data), _index(index)
118 virtual double getValue() const
119 { return _data->_imag[_index]; }
120 virtual void setValue(double value)
121 { _data->_imag[_index] = value; }
124 SGSharedPtr<HLACartesianLocation> _data;
128 class AngularVelocityDataElement : public HLAAbstractDoubleDataElement {
130 AngularVelocityDataElement(HLACartesianLocation* data, unsigned index) :
131 _data(data), _index(index)
133 virtual double getValue() const
134 { return _data->_angularVelocity[_index]; }
135 virtual void setValue(double value)
136 { _data->_angularVelocity[_index] = value; }
139 SGSharedPtr<HLACartesianLocation> _data;
143 class LinearVelocityDataElement : public HLAAbstractDoubleDataElement {
145 LinearVelocityDataElement(HLACartesianLocation* data, unsigned index) :
146 _data(data), _index(index)
148 virtual double getValue() const
149 { return _data->_linearVelocity[_index]; }
150 virtual void setValue(double value)
151 { _data->_linearVelocity[_index] = value; }
154 SGSharedPtr<HLACartesianLocation> _data;
161 SGVec3d _angularVelocity;
162 SGVec3d _linearVelocity;
165 class HLALocationFactory : public SGReferenced {
167 virtual ~HLALocationFactory() {}
168 virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap&) const = 0;
171 class HLACartesianLocationFactory : public HLALocationFactory {
173 virtual HLAAbstractLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
175 HLACartesianLocation* location = new HLACartesianLocation;
176 for (unsigned i = 0; i < 3; ++i) {
177 const HLADataElement::IndexPathPair& indexPathPair = _positonIndexPathPair[i];
178 attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getPositionDataElement(i);
180 for (unsigned i = 0; i < 3; ++i) {
181 const HLADataElement::IndexPathPair& indexPathPair = _orientationIndexPathPair[i];
182 attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getOrientationDataElement(i);
184 for (unsigned i = 0; i < 3; ++i) {
185 const HLADataElement::IndexPathPair& indexPathPair = _angularVelocityIndexPathPair[i];
186 attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getAngularVelocityDataElement(i);
188 for (unsigned i = 0; i < 3; ++i) {
189 const HLADataElement::IndexPathPair& indexPathPair = _linearVelocityIndexPathPair[i];
190 attributePathElementMap[indexPathPair.first][indexPathPair.second] = location->getLinearVelocityDataElement(i);
195 void setPositionIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
199 _positonIndexPathPair[index] = indexPathPair;
201 void setOrientationIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
205 _orientationIndexPathPair[index] = indexPathPair;
208 void setAngularVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
212 _angularVelocityIndexPathPair[index] = indexPathPair;
214 void setLinearVelocityIndexPathPair(unsigned index, const HLADataElement::IndexPathPair& indexPathPair)
218 _linearVelocityIndexPathPair[index] = indexPathPair;
222 HLADataElement::IndexPathPair _positonIndexPathPair[3];
223 HLADataElement::IndexPathPair _orientationIndexPathPair[3];
225 HLADataElement::IndexPathPair _angularVelocityIndexPathPair[3];
226 HLADataElement::IndexPathPair _linearVelocityIndexPathPair[3];
229 class HLAGeodeticLocation : public HLAAbstractLocation {
231 HLAGeodeticLocation() :
233 _cartPosition(SGVec3d::zeros()),
234 _cartOrientation(SGQuatd::unit()),
235 _cartBodyVelocity(SGVec3d::zeros()),
237 _geodEulerRad(SGVec3d::zeros()),
239 _groundSpeedMPerSec(0),
240 _verticalSpeedMPerSec(0)
242 updateCartesianFromGeodetic();
245 virtual SGVec3d getCartPosition() const
246 { updateCartesianFromGeodetic(); return _cartPosition; }
247 virtual void setCartPosition(const SGVec3d& position)
248 { _cartPosition = position; _dirty = true; }
250 virtual SGQuatd getCartOrientation() const
251 { updateCartesianFromGeodetic(); return _cartOrientation; }
252 virtual void setCartOrientation(const SGQuatd& orientation)
253 { _cartOrientation = orientation; _dirty = true; }
255 virtual SGVec3d getAngularBodyVelocity() const
256 { return SGVec3d::zeros(); }
257 virtual void setAngularBodyVelocity(const SGVec3d& angular)
260 virtual SGVec3d getLinearBodyVelocity() const
261 { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
262 virtual void setLinearBodyVelocity(const SGVec3d& linear)
263 { _cartBodyVelocity = linear; _dirty = true; }
265 void setLatitudeDeg(double value)
266 { _geodPosition.setLatitudeDeg(value); _dirty = true; }
267 double getLatitudeDeg() const
268 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
269 void setLatitudeRad(double value)
270 { _geodPosition.setLatitudeRad(value); _dirty = true; }
271 double getLatitudeRad() const
272 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
273 void setLongitudeDeg(double value)
274 { _geodPosition.setLongitudeDeg(value); _dirty = true; }
275 double getLongitudeDeg() const
276 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
277 void setLongitudeRad(double value)
278 { _geodPosition.setLongitudeRad(value); _dirty = true; }
279 double getLongitudeRad() const
280 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
281 void setElevationFt(double value)
282 { _geodPosition.setElevationFt(value); _dirty = true; }
283 double getElevationFt() const
284 { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
285 void setElevationM(double value)
286 { _geodPosition.setElevationM(value); _dirty = true; }
287 double getElevationM() const
288 { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
290 void setHeadingRad(double value)
291 { _geodEulerRad[2] = value; _dirty = true; }
292 double getHeadingRad() const
293 { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
294 void setHeadingDeg(double value)
295 { setHeadingRad(SGMiscd::deg2rad(value)); }
296 double getHeadingDeg() const
297 { return SGMiscd::rad2deg(getHeadingRad()); }
298 void setPitchRad(double value)
299 { _geodEulerRad[1] = value; _dirty = true; }
300 double getPitchRad() const
301 { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
302 void setPitchDeg(double value)
303 { setPitchRad(SGMiscd::deg2rad(value)); }
304 double getPitchDeg() const
305 { return SGMiscd::rad2deg(getPitchRad()); }
306 void setRollRad(double value)
307 { _geodEulerRad[0] = value; _dirty = true; }
308 double getRollRad() const
309 { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
310 void setRollDeg(double value)
311 { setRollRad(SGMiscd::deg2rad(value)); }
312 double getRollDeg() const
313 { return SGMiscd::rad2deg(getRollRad()); }
315 void setGroundTrackRad(double value)
316 { _groundTrackRad = value; _dirty = true; }
317 double getGroundTrackRad() const
318 { updateGeodeticFromCartesian(); return _groundTrackRad; }
319 void setGroundTrackDeg(double value)
320 { setGroundTrackRad(SGMiscd::deg2rad(value)); }
321 double getGroundTrackDeg() const
322 { return SGMiscd::rad2deg(getGroundTrackRad()); }
325 void setGroundSpeedMPerSec(double value)
326 { _groundSpeedMPerSec = value; _dirty = true; }
327 double getGroundSpeedMPerSec() const
328 { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
329 void setGroundSpeedFtPerSec(double value)
330 { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
331 double getGroundSpeedFtPerSec() const
332 { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
333 void setGroundSpeedKnots(double value)
334 { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
335 double getGroundSpeedKnots() const
336 { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
338 void setVerticalSpeedMPerSec(double value)
339 { _verticalSpeedMPerSec = value; _dirty = true; }
340 double getVerticalSpeedMPerSec() const
341 { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
342 void setVerticalSpeedFtPerSec(double value)
343 { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
344 double getVerticalSpeedFtPerSec() const
345 { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
346 void setVerticalSpeedFtPerMin(double value)
347 { setVerticalSpeedFtPerSec(value/60); }
348 double getVerticalSpeedFtPerMin() const
349 { return 60*getVerticalSpeedFtPerSec(); }
351 #define DATA_ELEMENT(name) \
352 HLADataElementProvider get## name ## DataElement() \
353 { return new DataElement<&HLAGeodeticLocation::get## name, &HLAGeodeticLocation::set ## name>(this); }
355 DATA_ELEMENT(LatitudeDeg)
356 DATA_ELEMENT(LatitudeRad)
357 DATA_ELEMENT(LongitudeDeg)
358 DATA_ELEMENT(LongitudeRad)
359 DATA_ELEMENT(ElevationFt)
360 DATA_ELEMENT(ElevationM)
361 DATA_ELEMENT(HeadingDeg)
362 DATA_ELEMENT(HeadingRad)
363 DATA_ELEMENT(PitchDeg)
364 DATA_ELEMENT(PitchRad)
365 DATA_ELEMENT(RollDeg)
366 DATA_ELEMENT(RollRad)
368 DATA_ELEMENT(GroundTrackDeg)
369 DATA_ELEMENT(GroundTrackRad)
370 DATA_ELEMENT(GroundSpeedMPerSec)
371 DATA_ELEMENT(GroundSpeedFtPerSec)
372 DATA_ELEMENT(GroundSpeedKnots)
373 DATA_ELEMENT(VerticalSpeedMPerSec)
374 DATA_ELEMENT(VerticalSpeedFtPerSec)
375 DATA_ELEMENT(VerticalSpeedFtPerMin)
380 template<double (HLAGeodeticLocation::*getter)() const,
381 void (HLAGeodeticLocation::*setter)(double)>
382 class DataElement : public HLAAbstractDoubleDataElement {
384 DataElement(HLAGeodeticLocation* data) :
387 virtual double getValue() const
388 { return (_data->*getter)(); }
389 virtual void setValue(double value)
390 { (_data->*setter)(value); }
393 SGSharedPtr<HLAGeodeticLocation> _data;
396 void updateGeodeticFromCartesian() const
400 _geodPosition = SGGeod::fromCart(_cartPosition);
401 SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
402 geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
403 SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
404 if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
405 _groundTrackRad = atan2(nedVel[1], nedVel[0]);
408 _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
409 _verticalSpeedMPerSec = -nedVel[2];
412 void updateCartesianFromGeodetic() const
416 _cartPosition = SGVec3d::fromGeod(_geodPosition);
417 SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
418 _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
419 SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
420 sin(_groundTrackRad)*_groundSpeedMPerSec,
421 -_verticalSpeedMPerSec);
422 _cartBodyVelocity = geodOrientation.transform(nedVel);
428 // the cartesian values
429 mutable SGVec3d _cartPosition;
430 mutable SGQuatd _cartOrientation;
431 mutable SGVec3d _cartBodyVelocity;
433 // The geodetic values
434 mutable SGGeod _geodPosition;
435 mutable SGVec3d _geodEulerRad;
436 mutable double _groundTrackRad;
437 mutable double _groundSpeedMPerSec;
438 mutable double _verticalSpeedMPerSec;
441 class HLAGeodeticLocationFactory : public HLALocationFactory {
461 VerticalSpeedFtPerSec,
462 VerticalSpeedFtPerMin,
466 virtual HLAGeodeticLocation* createLocation(HLAAttributePathElementMap& attributePathElementMap) const
468 HLAGeodeticLocation* location = new HLAGeodeticLocation;
470 for (IndexPathPairSemanticMap::const_iterator i = _indexPathPairSemanticMap.begin();
471 i != _indexPathPairSemanticMap.end(); ++i) {
474 attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeDegDataElement();
477 attributePathElementMap[i->first.first][i->first.second] = location->getLatitudeRadDataElement();
480 attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeDegDataElement();
483 attributePathElementMap[i->first.first][i->first.second] = location->getLongitudeRadDataElement();
486 attributePathElementMap[i->first.first][i->first.second] = location->getElevationMDataElement();
489 attributePathElementMap[i->first.first][i->first.second] = location->getElevationFtDataElement();
492 attributePathElementMap[i->first.first][i->first.second] = location->getHeadingDegDataElement();
495 attributePathElementMap[i->first.first][i->first.second] = location->getHeadingRadDataElement();
498 attributePathElementMap[i->first.first][i->first.second] = location->getPitchDegDataElement();
501 attributePathElementMap[i->first.first][i->first.second] = location->getPitchRadDataElement();
504 attributePathElementMap[i->first.first][i->first.second] = location->getRollDegDataElement();
507 attributePathElementMap[i->first.first][i->first.second] = location->getRollRadDataElement();
510 attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackDegDataElement();
513 attributePathElementMap[i->first.first][i->first.second] = location->getGroundTrackRadDataElement();
515 case GroundSpeedKnots:
516 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedKnotsDataElement();
518 case GroundSpeedFtPerSec:
519 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedFtPerSecDataElement();
521 case GroundSpeedMPerSec:
522 attributePathElementMap[i->first.first][i->first.second] = location->getGroundSpeedMPerSecDataElement();
524 case VerticalSpeedFtPerSec:
525 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerSecDataElement();
527 case VerticalSpeedFtPerMin:
528 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedFtPerMinDataElement();
530 case VerticalSpeedMPerSec:
531 attributePathElementMap[i->first.first][i->first.second] = location->getVerticalSpeedMPerSecDataElement();
539 void setIndexPathPair(Semantic semantic, const HLADataElement::IndexPathPair& indexPathPair)
540 { _indexPathPairSemanticMap[indexPathPair] = semantic; }
543 typedef std::map<HLADataElement::IndexPathPair, Semantic> IndexPathPairSemanticMap;
544 IndexPathPairSemanticMap _indexPathPairSemanticMap;
547 } // namespace simgear