simgear/math/SGLocation.hxx

   1 // Copyright (C) 2012  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   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.
   7 //
   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.
  12 //
  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.
  16 //
  17
  18 #ifndef SGLocation_HXX
  19 #define SGLocation_HXX
  20
  21 /// Encapsulates a pair SGVec3 and SGQuat.
  22 /// Together they encapsulate a cartesian position/orientation.
  23 /// Included are methods to do a simple euler position propagation step.
  24 template<typename T>
  25 class SGLocation {
  26 public:
  27     SGLocation(const SGVec3<T>& position = SGVec3<T>::zeros(),
  28                const SGQuat<T>& orientation = SGQuat<T>::unit()) :
  29         _position(position),
  30         _orientation(orientation)
  31     { }
  32
  33     const SGVec3<T>& getPosition() const
  34     { return _position; }
  35     void setPosition(const SGVec3<T>& position)
  36     { _position = position; }
  37
  38     const SGQuat<T>& getOrientation() const
  39     { return _orientation; }
  40     void setOrientation(const SGQuat<T>& orientation)
  41     { _orientation = orientation; }
  42
  43     /// Returns the absolute position of a relative position relative to this
  44     SGVec3<T> getAbsolutePosition(const SGVec3<T>& relativePosition) const
  45     { return getOrientation().backTransform(relativePosition) + _position; }
  46     /// Returns the relative position of an absolute position relative to this
  47     SGVec3<T> getRelativePosition(const SGVec3<T>& absolutePosition) const
  48     { return getOrientation().transform(absolutePosition) - _position; }
  49
  50     /// Returns the absolute orientation of a relative orientation relative to this
  51     SGQuat<T> getAbsoluteOrientation(const SGQuat<T>& relativeOrientation) const
  52     { return getOrientation()*relativeOrientation; }
  53     /// Returns the relative orientation of an absolute orientation relative to this
  54     SGQuat<T> getRelativeOrientation(const SGQuat<T>& absoluteOrientation) const
  55     { return inverse(getOrientation())*absoluteOrientation; }
  56
  57     /// Returns the absolute location of a relative location relative to this
  58     SGLocation getAbsoluteLocation(const SGLocation& relativeLocation) const
  59     {
  60         return SGLocation(getAbsolutePosition(relativeLocation.getPosition()),
  61                           getAbsoluteOrientation(relativeLocation.getOrientation()));
  62     }
  63
  64     /// Returns the relative location of an absolute location relative to this
  65     SGLocation getRelativeLocation(const SGLocation& absoluteLocation) const
  66     {
  67         return SGLocation(getRelativePosition(absoluteLocation.getPosition()),
  68                           getRelativeOrientation(absoluteLocation.getOrientation()));
  69     }
  70
  71     /// Executes an euler step with the given velocities in the current location
  72     void eulerStepBodyVelocities(const T& dt, const SGVec3<T>& linearBodyVelocity, const SGVec3<T>& angularBodyVelocity)
  73     {
  74         // Get the derivatives of the position and orientation due to the body velocities
  75         SGVec3<T> pDot = getOrientation().backTransform(linearBodyVelocity);
  76         SGQuat<T> qDot = getOrientation().derivative(angularBodyVelocity);
  77
  78         // and do the euler step.
  79         setPosition(getPosition() + dt*pDot);
  80         setOrientation(normalize(getOrientation() + dt*qDot));
  81     }
  82
  83     /// Executes an euler step with the given velocities in the current location,
  84     /// The position advance is here done with orientation in the middle of the orientation change.
  85     /// This leads to mostly correct extrapolations with rotating motion - even for longer times.
  86     void eulerStepBodyVelocitiesMidOrientation(const T& dt, const SGVec3<T>& linearBodyVelocity, const SGVec3<T>& angularBodyVelocity)
  87     {
  88         // Store the old orientation ...
  89         SGQuat<T> orientation = getOrientation();
  90         // ... and compute the new orientation
  91         SGQuat<T> qDot = orientation.derivative(angularBodyVelocity);
  92         setOrientation(normalize(orientation + dt*qDot));
  93
  94         // Then with the orientation in between, advance the position
  95         SGQuat<T> orientation05 = normalize(getOrientation() + orientation);
  96         SGVec3<T> pDot = orientation05.backTransform(linearBodyVelocity);
  97         setPosition(getPosition() + dt*pDot);
  98     }
  99
 100     /// Executes an euler step with the given velocities in the current location
 101     void eulerStepGlobalVelocities(const T& dt, const SGVec3<T>& linearVelocity, const SGVec3<T>& angularVelocity)
 102     {
 103         // Get the derivatives of the orientation in the body system
 104         SGVec3<T> angularBodyVelocity = getOrientation().transform(angularVelocity);
 105         SGQuat<T> qDot = getOrientation().derivative(angularBodyVelocity);
 106
 107         // and do the euler step.
 108         setPosition(getPosition() + dt*linearVelocity);
 109         setOrientation(normalize(getOrientation() + dt*qDot));
 110     }
 111
 112 private:
 113     SGVec3<T> _position;
 114     SGQuat<T> _orientation;
 115 };
 116
 117 #endif