1 // Copyright (C) 2012 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.
18 #ifndef SGLocation_HXX
19 #define SGLocation_HXX
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.
27 SGLocation(const SGVec3<T>& position = SGVec3<T>::zeros(),
28 const SGQuat<T>& orientation = SGQuat<T>::unit()) :
30 _orientation(orientation)
33 const SGVec3<T>& getPosition() const
35 void setPosition(const SGVec3<T>& position)
36 { _position = position; }
38 const SGQuat<T>& getOrientation() const
39 { return _orientation; }
40 void setOrientation(const SGQuat<T>& orientation)
41 { _orientation = orientation; }
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; }
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; }
57 /// Returns the absolute location of a relative location relative to this
58 SGLocation getAbsoluteLocation(const SGLocation& relativeLocation) const
60 return SGLocation(getAbsolutePosition(relativeLocation.getPosition()),
61 getAbsoluteOrientation(relativeLocation.getOrientation()));
64 /// Returns the relative location of an absolute location relative to this
65 SGLocation getRelativeLocation(const SGLocation& absoluteLocation) const
67 return SGLocation(getRelativePosition(absoluteLocation.getPosition()),
68 getRelativeOrientation(absoluteLocation.getOrientation()));
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)
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);
78 // and do the euler step.
79 setPosition(getPosition() + dt*pDot);
80 setOrientation(normalize(getOrientation() + dt*qDot));
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)
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));
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);
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)
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);
107 // and do the euler step.
108 setPosition(getPosition() + dt*linearVelocity);
109 setOrientation(normalize(getOrientation() + dt*qDot));
114 SGQuat<T> _orientation;