math: Implement SGLocation.

New simple class to encapsulate a cartesian position
and orientation pair with few handy methods.

simgear/math/CMakeLists.txt

@@ -13,6 +13,7 @@ set(HEADERS
     SGIntersect.hxx
     SGLimits.hxx
     SGLineSegment.hxx
+    SGLocation.hxx
     SGMath.hxx
     SGMathFwd.hxx
     SGMatrix.hxx

simgear/math/SGLocation.hxx

@@ -0,0 +1,117 @@
+// Copyright (C) 2012  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.
+//
+
+#ifndef SGLocation_HXX
+#define SGLocation_HXX
+
+/// Encapsulates a pair SGVec3 and SGQuat.
+/// Together they encapsulate a cartesian position/orientation.
+/// Included are methods to do a simple euler position propagation step.
+template<typename T>
+class SGLocation {
+public:
+    SGLocation(const SGVec3<T>& position = SGVec3<T>::zeros(),
+               const SGQuat<T>& orientation = SGQuat<T>::unit()) :
+        _position(position),
+        _orientation(orientation)
+    { }
+
+    const SGVec3<T>& getPosition() const
+    { return _position; }
+    void setPosition(const SGVec3<T>& position)
+    { _position = position; }
+
+    const SGQuat<T>& getOrientation() const
+    { return _orientation; }
+    void setOrientation(const SGQuat<T>& orientation)
+    { _orientation = orientation; }
+
+    /// Returns the absolute position of a relative position relative to this
+    SGVec3<T> getAbsolutePosition(const SGVec3<T>& relativePosition) const
+    { return getOrientation().backTransform(relativePosition) + _position; }
+    /// Returns the relative position of an absolute position relative to this
+    SGVec3<T> getRelativePosition(const SGVec3<T>& absolutePosition) const
+    { return getOrientation().transform(absolutePosition) - _position; }
+
+    /// Returns the absolute orientation of a relative orientation relative to this
+    SGQuat<T> getAbsoluteOrientation(const SGQuat<T>& relativeOrientation) const
+    { return getOrientation()*relativeOrientation; }
+    /// Returns the relative orientation of an absolute orientation relative to this
+    SGQuat<T> getRelativeOrientation(const SGQuat<T>& absoluteOrientation) const
+    { return inverse(getOrientation())*absoluteOrientation; }
+
+    /// Returns the absolute location of a relative location relative to this
+    SGLocation getAbsoluteLocation(const SGLocation& relativeLocation) const
+    {
+        return SGLocation(getAbsolutePosition(relativeLocation.getPosition()),
+                          getAbsoluteOrientation(relativeLocation.getOrientation()));
+    }
+
+    /// Returns the relative location of an absolute location relative to this
+    SGLocation getRelativeLocation(const SGLocation& absoluteLocation) const
+    {
+        return SGLocation(getRelativePosition(absoluteLocation.getPosition()),
+                          getRelativeOrientation(absoluteLocation.getOrientation()));
+    }
+
+    /// Executes an euler step with the given velocities in the current location
+    void eulerStepBodyVelocities(const T& dt, const SGVec3<T>& linearBodyVelocity, const SGVec3<T>& angularBodyVelocity)
+    {
+        // Get the derivatives of the position and orientation due to the body velocities
+        SGVec3<T> pDot = getOrientation().backTransform(linearBodyVelocity);
+        SGQuat<T> qDot = getOrientation().derivative(angularBodyVelocity);
+
+        // and do the euler step.
+        setPosition(getPosition() + dt*pDot);
+        setOrientation(normalize(getOrientation() + dt*qDot));
+    }
+
+    /// Executes an euler step with the given velocities in the current location,
+    /// The position advance is here done with orientation in the middle of the orientation change.
+    /// This leads to mostly correct extrapolations with rotating motion - even for longer times.
+    void eulerStepBodyVelocitiesMidOrientation(const T& dt, const SGVec3<T>& linearBodyVelocity, const SGVec3<T>& angularBodyVelocity)
+    {
+        // Store the old orientation ...
+        SGQuat<T> orientation = getOrientation();
+        // ... and compute the new orientation
+        SGQuat<T> qDot = orientation.derivative(angularBodyVelocity);
+        setOrientation(normalize(orientation + dt*qDot));
+
+        // Then with the orientation in between, advance the position
+        SGQuat<T> orientation05 = normalize(getOrientation() + orientation);
+        SGVec3<T> pDot = orientation05.backTransform(linearBodyVelocity);
+        setPosition(getPosition() + dt*pDot);
+    }
+
+    /// Executes an euler step with the given velocities in the current location
+    void eulerStepGlobalVelocities(const T& dt, const SGVec3<T>& linearVelocity, const SGVec3<T>& angularVelocity)
+    {
+        // Get the derivatives of the orientation in the body system
+        SGVec3<T> angularBodyVelocity = getOrientation().transform(angularVelocity);
+        SGQuat<T> qDot = getOrientation().derivative(angularBodyVelocity);
+
+        // and do the euler step.
+        setPosition(getPosition() + dt*linearVelocity);
+        setOrientation(normalize(getOrientation() + dt*qDot));
+    }
+
+private:
+    SGVec3<T> _position;
+    SGQuat<T> _orientation;
+};
+
+#endif

simgear/math/SGMath.hxx

@@ -34,6 +34,7 @@
 #include "SGGeoc.hxx"
 #include "SGGeod.hxx"
 #include "SGQuat.hxx"
+#include "SGLocation.hxx"
 #include "SGMatrix.hxx"
 
 #endif

simgear/math/SGMathFwd.hxx

@@ -23,6 +23,8 @@
 class SGGeoc;
 class SGGeod;
 
+template<typename T>
+class SGLocation;
 template<typename T>
 class SGLimits;
 template<typename T>
@@ -38,6 +40,8 @@ class SGVec3;
 template<typename T>
 class SGVec4;
 
+typedef SGLocation<float> SGLocationf;
+typedef SGLocation<double> SGLocationd;
 typedef SGLimits<float> SGLimitsf;
 typedef SGLimits<double> SGLimitsd;
 typedef SGMatrix<float> SGMatrixf;