hla: Make use of SGLocation.

diff --git a/simgear/hla/HLALocation.hxx b/simgear/hla/HLALocation.hxx
index 474a39c6d216635bb76a70f0371cafb49b94f135..a43f516062369d21accec5fea3a541270871d5b4 100644 (file)
--- a/simgear/hla/HLALocation.hxx
+++ b/simgear/hla/HLALocation.hxx
@@ -18,6 +18,7 @@
 #ifndef HLALocation_hxx
 #define HLALocation_hxx
 
+#include <simgear/math/SGMath.hxx>
 #include "HLABasicDataElement.hxx"
 #include "HLATypes.hxx"
 
@@ -27,6 +28,9 @@ class HLAAbstractLocation : public SGReferenced {
 public:
     virtual ~HLAAbstractLocation() {}
 
+    virtual SGLocationd getLocation() const = 0;
+    virtual void setLocation(const SGLocationd&) = 0;
+
     virtual SGVec3d getCartPosition() const = 0;
     virtual void setCartPosition(const SGVec3d&) = 0;
 
@@ -34,10 +38,21 @@ public:
     virtual void setCartOrientation(const SGQuatd&) = 0;
 
     virtual SGVec3d getAngularBodyVelocity() const = 0;
-    virtual void setAngularBodyVelocity(const SGVec3d& angular) = 0;
+    virtual void setAngularBodyVelocity(const SGVec3d&) = 0;
 
     virtual SGVec3d getLinearBodyVelocity() const = 0;
-    virtual void setLinearBodyVelocity(const SGVec3d& linear) = 0;
+    virtual void setLinearBodyVelocity(const SGVec3d&) = 0;
+
+    virtual double getTimeDifference(const SGTimeStamp&) const
+    { return 0; }
+
+    // Get the position and orientation extrapolated to the given time stamp.
+    SGLocationd getLocation(const SGTimeStamp& timeStamp) const
+    {
+        SGLocationd location = getLocation();
+        location.eulerStepBodyVelocities(getTimeDifference(timeStamp), getLinearBodyVelocity(), getAngularBodyVelocity());
+        return location;
+    }
 };
 
 class HLACartesianLocation : public HLAAbstractLocation {
@@ -49,6 +64,14 @@ public:
         _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();
+    }
+
     virtual SGVec3d getCartPosition() const
     { return _position; }
     virtual void setCartPosition(const SGVec3d& position)
@@ -260,6 +283,18 @@ public:
         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)