From: Mathias Froehlich Date: Sat, 24 Nov 2012 08:54:21 +0000 (+0100) Subject: math: Implement SGLocation. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=f892d88c10a137ef45c7519b001199b63d1735ef;p=simgear.git math: Implement SGLocation. New simple class to encapsulate a cartesian position and orientation pair with few handy methods. --- diff --git a/simgear/math/CMakeLists.txt b/simgear/math/CMakeLists.txt index a334680d..1308f1f4 100644 --- a/simgear/math/CMakeLists.txt +++ b/simgear/math/CMakeLists.txt @@ -13,6 +13,7 @@ set(HEADERS SGIntersect.hxx SGLimits.hxx SGLineSegment.hxx + SGLocation.hxx SGMath.hxx SGMathFwd.hxx SGMatrix.hxx diff --git a/simgear/math/SGLocation.hxx b/simgear/math/SGLocation.hxx new file mode 100644 index 00000000..da047dd7 --- /dev/null +++ b/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 +class SGLocation { +public: + SGLocation(const SGVec3& position = SGVec3::zeros(), + const SGQuat& orientation = SGQuat::unit()) : + _position(position), + _orientation(orientation) + { } + + const SGVec3& getPosition() const + { return _position; } + void setPosition(const SGVec3& position) + { _position = position; } + + const SGQuat& getOrientation() const + { return _orientation; } + void setOrientation(const SGQuat& orientation) + { _orientation = orientation; } + + /// Returns the absolute position of a relative position relative to this + SGVec3 getAbsolutePosition(const SGVec3& relativePosition) const + { return getOrientation().backTransform(relativePosition) + _position; } + /// Returns the relative position of an absolute position relative to this + SGVec3 getRelativePosition(const SGVec3& absolutePosition) const + { return getOrientation().transform(absolutePosition) - _position; } + + /// Returns the absolute orientation of a relative orientation relative to this + SGQuat getAbsoluteOrientation(const SGQuat& relativeOrientation) const + { return getOrientation()*relativeOrientation; } + /// Returns the relative orientation of an absolute orientation relative to this + SGQuat getRelativeOrientation(const SGQuat& 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& linearBodyVelocity, const SGVec3& angularBodyVelocity) + { + // Get the derivatives of the position and orientation due to the body velocities + SGVec3 pDot = getOrientation().backTransform(linearBodyVelocity); + SGQuat 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& linearBodyVelocity, const SGVec3& angularBodyVelocity) + { + // Store the old orientation ... + SGQuat orientation = getOrientation(); + // ... and compute the new orientation + SGQuat qDot = orientation.derivative(angularBodyVelocity); + setOrientation(normalize(orientation + dt*qDot)); + + // Then with the orientation in between, advance the position + SGQuat orientation05 = normalize(getOrientation() + orientation); + SGVec3 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& linearVelocity, const SGVec3& angularVelocity) + { + // Get the derivatives of the orientation in the body system + SGVec3 angularBodyVelocity = getOrientation().transform(angularVelocity); + SGQuat qDot = getOrientation().derivative(angularBodyVelocity); + + // and do the euler step. + setPosition(getPosition() + dt*linearVelocity); + setOrientation(normalize(getOrientation() + dt*qDot)); + } + +private: + SGVec3 _position; + SGQuat _orientation; +}; + +#endif diff --git a/simgear/math/SGMath.hxx b/simgear/math/SGMath.hxx index e33fe3ac..b311136b 100644 --- a/simgear/math/SGMath.hxx +++ b/simgear/math/SGMath.hxx @@ -34,6 +34,7 @@ #include "SGGeoc.hxx" #include "SGGeod.hxx" #include "SGQuat.hxx" +#include "SGLocation.hxx" #include "SGMatrix.hxx" #endif diff --git a/simgear/math/SGMathFwd.hxx b/simgear/math/SGMathFwd.hxx index 5fe19954..3570aa40 100644 --- a/simgear/math/SGMathFwd.hxx +++ b/simgear/math/SGMathFwd.hxx @@ -23,6 +23,8 @@ class SGGeoc; class SGGeod; +template +class SGLocation; template class SGLimits; template @@ -38,6 +40,8 @@ class SGVec3; template class SGVec4; +typedef SGLocation SGLocationf; +typedef SGLocation SGLocationd; typedef SGLimits SGLimitsf; typedef SGLimits SGLimitsd; typedef SGMatrix SGMatrixf;