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.
19 # include <simgear_config.h>
22 #include <simgear/compiler.h>
24 #include "HLALocation.hxx"
26 #include "HLAArrayDataElement.hxx"
27 #include "HLAObjectInstance.hxx"
31 HLAAbstractLocationFactory::~HLAAbstractLocationFactory()
35 ////////////////////////////////////////////////////////////////////////
37 HLACartesianLocation::HLACartesianLocation()
41 HLACartesianLocation::~HLACartesianLocation()
46 HLACartesianLocation::getLocation() const
48 return SGLocationd(getCartPosition(), getCartOrientation());
52 HLACartesianLocation::setLocation(const SGLocationd& location)
54 setCartPosition(location.getPosition());
55 setCartOrientation(location.getOrientation());
59 HLACartesianLocation::getCartPosition() const
61 return SGVec3d(_position[0].getValue(), _position[1].getValue(), _position[2].getValue());
65 HLACartesianLocation::setCartPosition(const SGVec3d& position)
67 for (unsigned i = 0; i < 3; ++i)
68 _position[i].setValue(position[i]);
72 HLACartesianLocation::getCartOrientation() const
74 return SGQuatd::fromPositiveRealImag(SGVec3d(_imag[0].getValue(), _imag[1].getValue(), _imag[2].getValue()));
78 HLACartesianLocation::setCartOrientation(const SGQuatd& orientation)
80 SGVec3d imag = orientation.getPositiveRealImag();
81 for (unsigned i = 0; i < 3; ++i)
82 _imag[i].setValue(imag[i]);
86 HLACartesianLocation::getAngularBodyVelocity() const
88 return SGVec3d(_angularVelocity[0].getValue(), _angularVelocity[1].getValue(), _angularVelocity[2].getValue());
92 HLACartesianLocation::setAngularBodyVelocity(const SGVec3d& angularVelocity)
94 for (unsigned i = 0; i < 3; ++i)
95 _angularVelocity[i].setValue(angularVelocity[i]);
99 HLACartesianLocation::getLinearBodyVelocity() const
101 return SGVec3d(_linearVelocity[0].getValue(), _linearVelocity[1].getValue(), _linearVelocity[2].getValue());
105 HLACartesianLocation::setLinearBodyVelocity(const SGVec3d& linearVelocity)
107 for (unsigned i = 0; i < 3; ++i)
108 _linearVelocity[i].setValue(linearVelocity[i]);
112 HLACartesianLocation::getTimeDifference(const SGTimeStamp& timeStamp) const
114 return _position[0].getDataElement()->getTimeDifference(timeStamp);
118 HLACartesianLocation::getPositionDataElement()
120 HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
121 for (unsigned i = 0; i < 3; ++i)
122 arrayDataElement->setElement(i, _position[i].getDataElement());
123 return arrayDataElement;
127 HLACartesianLocation::getPositionDataElement(unsigned i)
131 return _position[i].getDataElement();
135 HLACartesianLocation::getOrientationDataElement()
137 HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
138 for (unsigned i = 0; i < 3; ++i)
139 arrayDataElement->setElement(i, _imag[i].getDataElement());
140 return arrayDataElement;
144 HLACartesianLocation::getOrientationDataElement(unsigned i)
148 return _imag[i].getDataElement();
152 HLACartesianLocation::getAngularVelocityDataElement()
154 HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
155 for (unsigned i = 0; i < 3; ++i)
156 arrayDataElement->setElement(i, _angularVelocity[i].getDataElement());
157 return arrayDataElement;
161 HLACartesianLocation::getAngularVelocityDataElement(unsigned i)
165 return _angularVelocity[i].getDataElement();
169 HLACartesianLocation::getLinearVelocityDataElement()
171 HLAArrayDataElement* arrayDataElement = new HLAArrayDataElement;
172 for (unsigned i = 0; i < 3; ++i)
173 arrayDataElement->setElement(i, _linearVelocity[i].getDataElement());
174 return arrayDataElement;
178 HLACartesianLocation::getLinearVelocityDataElement(unsigned i)
182 return _linearVelocity[i].getDataElement();
185 ////////////////////////////////////////////////////////////////////////
187 HLACartesianLocationFactory::HLACartesianLocationFactory()
191 HLACartesianLocationFactory::~HLACartesianLocationFactory()
195 HLACartesianLocation*
196 HLACartesianLocationFactory::createLocation(HLAObjectInstance& objectInstance) const
198 HLACartesianLocation* location = new HLACartesianLocation;
200 for (unsigned i = 0; i < 3; ++i)
201 objectInstance.setAttributeDataElement(_positionIndex[i], location->getPositionDataElement(i));
202 for (unsigned i = 0; i < 3; ++i)
203 objectInstance.setAttributeDataElement(_orientationIndex[i], location->getOrientationDataElement(i));
204 for (unsigned i = 0; i < 3; ++i)
205 objectInstance.setAttributeDataElement(_angularVelocityIndex[i], location->getAngularVelocityDataElement(i));
206 for (unsigned i = 0; i < 3; ++i)
207 objectInstance.setAttributeDataElement(_linearVelocityIndex[i], location->getLinearVelocityDataElement(i));
213 HLACartesianLocationFactory::setPositionIndex(const HLADataElementIndex& dataElementIndex)
215 for (unsigned i = 0; i < 3; ++i) {
216 _positionIndex[i] = dataElementIndex;
217 _positionIndex[i].push_back(i);
222 HLACartesianLocationFactory::setPositionIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
226 _positionIndex[index] = dataElementIndex;
230 HLACartesianLocationFactory::setOrientationIndex(const HLADataElementIndex& dataElementIndex)
232 for (unsigned i = 0; i < 3; ++i) {
233 _orientationIndex[i] = dataElementIndex;
234 _orientationIndex[i].push_back(i);
239 HLACartesianLocationFactory::setOrientationIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
243 _orientationIndex[index] = dataElementIndex;
247 HLACartesianLocationFactory::setAngularVelocityIndex(const HLADataElementIndex& dataElementIndex)
249 for (unsigned i = 0; i < 3; ++i) {
250 _angularVelocityIndex[i] = dataElementIndex;
251 _angularVelocityIndex[i].push_back(i);
256 HLACartesianLocationFactory::setAngularVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
260 _angularVelocityIndex[index] = dataElementIndex;
264 HLACartesianLocationFactory::setLinearVelocityIndex(const HLADataElementIndex& dataElementIndex)
266 for (unsigned i = 0; i < 3; ++i) {
267 _linearVelocityIndex[i] = dataElementIndex;
268 _linearVelocityIndex[i].push_back(i);
273 HLACartesianLocationFactory::setLinearVelocityIndex(unsigned index, const HLADataElementIndex& dataElementIndex)
277 _linearVelocityIndex[index] = dataElementIndex;
280 ////////////////////////////////////////////////////////////////////////
282 class HLAGeodeticLocationFactory::Location : public HLAAbstractLocation {
286 _cartPosition(SGVec3d::zeros()),
287 _cartOrientation(SGQuatd::unit()),
288 _cartBodyVelocity(SGVec3d::zeros()),
290 _geodEulerRad(SGVec3d::zeros()),
292 _groundSpeedMPerSec(0),
293 _verticalSpeedMPerSec(0)
295 updateCartesianFromGeodetic();
301 virtual SGLocationd getLocation() const
303 updateCartesianFromGeodetic();
304 return SGLocationd(_cartPosition, _cartOrientation);
306 virtual void setLocation(const SGLocationd& location)
308 _cartPosition = location.getPosition();
309 _cartOrientation = location.getOrientation();
313 virtual SGVec3d getCartPosition() const
314 { updateCartesianFromGeodetic(); return _cartPosition; }
315 virtual void setCartPosition(const SGVec3d& position)
316 { _cartPosition = position; _dirty = true; }
318 virtual SGQuatd getCartOrientation() const
319 { updateCartesianFromGeodetic(); return _cartOrientation; }
320 virtual void setCartOrientation(const SGQuatd& orientation)
321 { _cartOrientation = orientation; _dirty = true; }
323 virtual SGVec3d getAngularBodyVelocity() const
324 { return SGVec3d::zeros(); }
325 virtual void setAngularBodyVelocity(const SGVec3d& angular)
328 virtual SGVec3d getLinearBodyVelocity() const
329 { updateCartesianFromGeodetic(); return _cartBodyVelocity; }
330 virtual void setLinearBodyVelocity(const SGVec3d& linear)
331 { _cartBodyVelocity = linear; _dirty = true; }
333 void setLatitudeDeg(double value)
334 { _geodPosition.setLatitudeDeg(value); _dirty = true; }
335 double getLatitudeDeg() const
336 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeDeg(); }
337 void setLatitudeRad(double value)
338 { _geodPosition.setLatitudeRad(value); _dirty = true; }
339 double getLatitudeRad() const
340 { updateGeodeticFromCartesian(); return _geodPosition.getLatitudeRad(); }
341 void setLongitudeDeg(double value)
342 { _geodPosition.setLongitudeDeg(value); _dirty = true; }
343 double getLongitudeDeg() const
344 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeDeg(); }
345 void setLongitudeRad(double value)
346 { _geodPosition.setLongitudeRad(value); _dirty = true; }
347 double getLongitudeRad() const
348 { updateGeodeticFromCartesian(); return _geodPosition.getLongitudeRad(); }
349 void setElevationFt(double value)
350 { _geodPosition.setElevationFt(value); _dirty = true; }
351 double getElevationFt() const
352 { updateGeodeticFromCartesian(); return _geodPosition.getElevationFt(); }
353 void setElevationM(double value)
354 { _geodPosition.setElevationM(value); _dirty = true; }
355 double getElevationM() const
356 { updateGeodeticFromCartesian(); return _geodPosition.getElevationM(); }
358 void setHeadingRad(double value)
359 { _geodEulerRad[2] = value; _dirty = true; }
360 double getHeadingRad() const
361 { updateGeodeticFromCartesian(); return _geodEulerRad[2]; }
362 void setHeadingDeg(double value)
363 { setHeadingRad(SGMiscd::deg2rad(value)); }
364 double getHeadingDeg() const
365 { return SGMiscd::rad2deg(getHeadingRad()); }
366 void setPitchRad(double value)
367 { _geodEulerRad[1] = value; _dirty = true; }
368 double getPitchRad() const
369 { updateGeodeticFromCartesian(); return _geodEulerRad[1]; }
370 void setPitchDeg(double value)
371 { setPitchRad(SGMiscd::deg2rad(value)); }
372 double getPitchDeg() const
373 { return SGMiscd::rad2deg(getPitchRad()); }
374 void setRollRad(double value)
375 { _geodEulerRad[0] = value; _dirty = true; }
376 double getRollRad() const
377 { updateGeodeticFromCartesian(); return _geodEulerRad[0]; }
378 void setRollDeg(double value)
379 { setRollRad(SGMiscd::deg2rad(value)); }
380 double getRollDeg() const
381 { return SGMiscd::rad2deg(getRollRad()); }
383 void setGroundTrackRad(double value)
384 { _groundTrackRad = value; _dirty = true; }
385 double getGroundTrackRad() const
386 { updateGeodeticFromCartesian(); return _groundTrackRad; }
387 void setGroundTrackDeg(double value)
388 { setGroundTrackRad(SGMiscd::deg2rad(value)); }
389 double getGroundTrackDeg() const
390 { return SGMiscd::rad2deg(getGroundTrackRad()); }
393 void setGroundSpeedMPerSec(double value)
394 { _groundSpeedMPerSec = value; _dirty = true; }
395 double getGroundSpeedMPerSec() const
396 { updateGeodeticFromCartesian(); return _groundSpeedMPerSec; }
397 void setGroundSpeedFtPerSec(double value)
398 { setGroundSpeedMPerSec(SG_FEET_TO_METER*value); }
399 double getGroundSpeedFtPerSec() const
400 { return SG_METER_TO_FEET*getGroundSpeedMPerSec(); }
401 void setGroundSpeedKnots(double value)
402 { setGroundSpeedMPerSec(SG_KT_TO_MPS*value); }
403 double getGroundSpeedKnots() const
404 { return SG_MPS_TO_KT*getGroundSpeedMPerSec(); }
406 void setVerticalSpeedMPerSec(double value)
407 { _verticalSpeedMPerSec = value; _dirty = true; }
408 double getVerticalSpeedMPerSec() const
409 { updateGeodeticFromCartesian(); return _verticalSpeedMPerSec; }
410 void setVerticalSpeedFtPerSec(double value)
411 { setVerticalSpeedMPerSec(SG_FEET_TO_METER*value); }
412 double getVerticalSpeedFtPerSec() const
413 { return SG_METER_TO_FEET*getVerticalSpeedMPerSec(); }
414 void setVerticalSpeedFtPerMin(double value)
415 { setVerticalSpeedFtPerSec(value/60); }
416 double getVerticalSpeedFtPerMin() const
417 { return 60*getVerticalSpeedFtPerSec(); }
419 #define DATA_ELEMENT(name) \
420 HLADataElement* get## name ## DataElement() \
421 { return new DataElement<&Location::get## name, &Location::set ## name>(this); }
423 DATA_ELEMENT(LatitudeDeg)
424 DATA_ELEMENT(LatitudeRad)
425 DATA_ELEMENT(LongitudeDeg)
426 DATA_ELEMENT(LongitudeRad)
427 DATA_ELEMENT(ElevationFt)
428 DATA_ELEMENT(ElevationM)
429 DATA_ELEMENT(HeadingDeg)
430 DATA_ELEMENT(HeadingRad)
431 DATA_ELEMENT(PitchDeg)
432 DATA_ELEMENT(PitchRad)
433 DATA_ELEMENT(RollDeg)
434 DATA_ELEMENT(RollRad)
436 DATA_ELEMENT(GroundTrackDeg)
437 DATA_ELEMENT(GroundTrackRad)
438 DATA_ELEMENT(GroundSpeedMPerSec)
439 DATA_ELEMENT(GroundSpeedFtPerSec)
440 DATA_ELEMENT(GroundSpeedKnots)
441 DATA_ELEMENT(VerticalSpeedMPerSec)
442 DATA_ELEMENT(VerticalSpeedFtPerSec)
443 DATA_ELEMENT(VerticalSpeedFtPerMin)
447 HLADataElement* getDataElement(Semantic semantic)
451 return getLatitudeDegDataElement();
453 return getLatitudeRadDataElement();
455 return getLongitudeDegDataElement();
457 return getLongitudeRadDataElement();
459 return getElevationMDataElement();
461 return getElevationFtDataElement();
463 return getHeadingDegDataElement();
465 return getHeadingRadDataElement();
467 return getPitchDegDataElement();
469 return getPitchRadDataElement();
471 return getRollDegDataElement();
473 return getRollRadDataElement();
475 return getGroundTrackDegDataElement();
477 return getGroundTrackRadDataElement();
478 case GroundSpeedKnots:
479 return getGroundSpeedKnotsDataElement();
480 case GroundSpeedFtPerSec:
481 return getGroundSpeedFtPerSecDataElement();
482 case GroundSpeedMPerSec:
483 return getGroundSpeedMPerSecDataElement();
484 case VerticalSpeedFtPerSec:
485 return getVerticalSpeedFtPerSecDataElement();
486 case VerticalSpeedFtPerMin:
487 return getVerticalSpeedFtPerMinDataElement();
488 case VerticalSpeedMPerSec:
489 return getVerticalSpeedMPerSecDataElement();
496 template<double (Location::*getter)() const, void (Location::*setter)(double)>
497 class DataElement : public HLAAbstractDoubleDataElement {
499 DataElement(Location* data) :
502 virtual double getValue() const
503 { return (_data->*getter)(); }
504 virtual void setValue(double value)
505 { (_data->*setter)(value); }
508 SGSharedPtr<Location> _data;
511 void updateGeodeticFromCartesian() const
515 _geodPosition = SGGeod::fromCart(_cartPosition);
516 SGQuatd geodOrientation = inverse(SGQuatd::fromLonLat(_geodPosition))*_cartOrientation;
517 geodOrientation.getEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
518 SGVec3d nedVel = geodOrientation.backTransform(_cartBodyVelocity);
519 if (SGLimitsd::min() < SGMiscd::max(fabs(nedVel[0]), fabs(nedVel[1])))
520 _groundTrackRad = atan2(nedVel[1], nedVel[0]);
523 _groundSpeedMPerSec = sqrt(nedVel[0]*nedVel[0] + nedVel[1]*nedVel[1]);
524 _verticalSpeedMPerSec = -nedVel[2];
527 void updateCartesianFromGeodetic() const
531 _cartPosition = SGVec3d::fromGeod(_geodPosition);
532 SGQuatd geodOrientation = SGQuatd::fromEulerRad(_geodEulerRad[2], _geodEulerRad[1], _geodEulerRad[0]);
533 _cartOrientation = SGQuatd::fromLonLat(_geodPosition)*geodOrientation;
534 SGVec3d nedVel(cos(_groundTrackRad)*_groundSpeedMPerSec,
535 sin(_groundTrackRad)*_groundSpeedMPerSec,
536 -_verticalSpeedMPerSec);
537 _cartBodyVelocity = geodOrientation.transform(nedVel);
543 // the cartesian values
544 mutable SGVec3d _cartPosition;
545 mutable SGQuatd _cartOrientation;
546 mutable SGVec3d _cartBodyVelocity;
548 // The geodetic values
549 mutable SGGeod _geodPosition;
550 mutable SGVec3d _geodEulerRad;
551 mutable double _groundTrackRad;
552 mutable double _groundSpeedMPerSec;
553 mutable double _verticalSpeedMPerSec;
556 HLAGeodeticLocationFactory::HLAGeodeticLocationFactory()
560 HLAGeodeticLocationFactory::~HLAGeodeticLocationFactory()
565 HLAGeodeticLocationFactory::createLocation(HLAObjectInstance& objectInstance) const
567 Location* location = new Location;
569 for (IndexSemanticMap::const_iterator i = _indexSemanticMap.begin();
570 i != _indexSemanticMap.end(); ++i) {
571 Semantic semantic = i->second;
572 objectInstance.setAttributeDataElement(i->first, location->getDataElement(semantic));
578 } // namespace simgear