Author: Jon Berndt, Mathis Froehlich
Date started: 12/02/98
- ------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------------
+ ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------------
------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
This program is free software; you can redistribute it and/or modify it under
- the terms of the GNU General Public License as published by the Free Software
+ the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program 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 General Public License for more
+ FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
- You should have received a copy of the GNU General Public License along with
+ You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
- Further information about the GNU General Public License can also be found on
+ Further information about the GNU Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#include <FGJSBBase.h>
-#include "FGMatrix33.h"
+#include "FGJSBBase.h"
#include "FGColumnVector3.h"
-#include <input_output/FGPropertyManager.h>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_QUATERNION "$Id$"
+#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.17 2010/06/30 03:13:40 jberndt Exp $"
namespace JSBSim {
+class FGMatrix33;
+
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-class FGQuaternion
- : virtual FGJSBBase {
+class FGQuaternion : virtual FGJSBBase {
public:
/** Default initializer.
Default initializer, initializes the class with the identity rotation. */
FGQuaternion() : mCacheValid(false) {
- Entry(1) = 1.0;
- Entry(2) = Entry(3) = Entry(4) = 0.0;
+ data[0] = 1.0;
+ data[1] = data[2] = data[3] = 0.0;
}
/** Copy constructor.
@param psi The euler Z axis (heading) angle in radians */
FGQuaternion(double phi, double tht, double psi);
+ /** Initializer by euler angle vector.
+ Initialize the quaternion with the euler angle vector.
+ @param vOrient The euler axis angle vector in radians (phi, tht, psi) */
+ FGQuaternion(FGColumnVector3 vOrient);
+
/** Initializer by one euler angle.
Initialize the quaternion with the single euler angle where its index
is given in the first argument.
@param angle The euler angle in radians */
FGQuaternion(int idx, double angle)
: mCacheValid(false) {
+
double angle2 = 0.5*angle;
double Sangle2 = sin(angle2);
double Cangle2 = cos(angle2);
if (idx == ePhi) {
- Entry(1) = Cangle2;
- Entry(2) = Sangle2;
- Entry(3) = 0.0;
- Entry(4) = 0.0;
+ data[0] = Cangle2;
+ data[1] = Sangle2;
+ data[2] = 0.0;
+ data[3] = 0.0;
} else if (idx == eTht) {
- Entry(1) = Cangle2;
- Entry(2) = 0.0;
- Entry(3) = Sangle2;
- Entry(4) = 0.0;
+ data[0] = Cangle2;
+ data[1] = 0.0;
+ data[2] = Sangle2;
+ data[3] = 0.0;
} else {
- Entry(1) = Cangle2;
- Entry(2) = 0.0;
- Entry(3) = 0.0;
- Entry(4) = Sangle2;
+ data[0] = Cangle2;
+ data[1] = 0.0;
+ data[2] = 0.0;
+ data[3] = Sangle2;
}
}
+ /** Initializer by matrix.
+ Initialize the quaternion with the matrix representing a transform from one frame
+ to another using the standard aerospace sequence, Yaw-Pitch-Roll (3-2-1).
+ @param m the rotation matrix */
+ FGQuaternion(const FGMatrix33& m);
+
/// Destructor.
~FGQuaternion() {}
- /** Quaternion 'velocity' for given angular rates.
+ /** Quaternion derivative for given angular rates.
Computes the quaternion derivative which results from the given
angular velocities
- @param PQR a constant reference to the body rate vector
- @return the quaternion derivative */
- FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
+ @param PQR a constant reference to a rotation rate vector
+ @return the quaternion derivative
+ @see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
+ Equation 1.3-36. */
+ FGQuaternion GetQDot(const FGColumnVector3& PQR);
/** Transformation matrix.
@return a reference to the transformation/rotation matrix
/** Retrieves the Euler angles.
@return a reference to the triad of euler angles corresponding
to this quaternion rotation.
- @units radians */
+ units radians */
const FGColumnVector3& GetEuler(void) const {
ComputeDerived();
return mEulerAngles;
/** Retrieves the Euler angles.
@param i the euler angle index.
+ units radians.
@return a reference to the i-th euler angles corresponding
to this quaternion rotation.
- @units radians */
+ */
double GetEuler(int i) const {
ComputeDerived();
return mEulerAngles(i);
@param i the euler angle index.
@return a reference to the i-th euler angles corresponding
to this quaternion rotation.
- @units degrees */
+ units degrees */
double GetEulerDeg(int i) const {
ComputeDerived();
return radtodeg*mEulerAngles(i);
Note that the index given in the argument is unchecked.
*/
- double operator()(unsigned int idx) const { return Entry(idx); }
+ double operator()(unsigned int idx) const { return data[idx-1]; }
/** Write access the entries of the vector.
Note that the index given in the argument is unchecked.
*/
- double& operator()(unsigned int idx) { return Entry(idx); }
+ double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
/** Read access the entries of the vector.
Return the value of the matrix entry at the given index.
Indices are counted starting with 1.
- This function is just a shortcut for the @ref double
- operator()(unsigned int idx) const function. It is
+ This function is just a shortcut for the <tt>double
+ operator()(unsigned int idx) const</tt> function. It is
used internally to access the elements in a more convenient way.
Note that the index given in the argument is unchecked.
*/
- double Entry(unsigned int idx) const { return mData[idx-1]; }
+ double Entry(unsigned int idx) const { return data[idx-1]; }
/** Write access the entries of the vector.
Return a reference to the vector entry at the given index.
Indices are counted starting with 1.
- This function is just a shortcut for the @ref double&
- operator()(unsigned int idx) function. It is
+ This function is just a shortcut for the <tt>double&
+ operator()(unsigned int idx)</tt> function. It is
used internally to access the elements in a more convenient way.
Note that the index given in the argument is unchecked.
*/
- double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
+ double& Entry(unsigned int idx) {
+ mCacheValid = false;
+ return data[idx-1];
+ }
/** Assignment operator "=".
Assign the value of q to the current object. Cached values are
@return reference to a quaternion object */
const FGQuaternion& operator=(const FGQuaternion& q) {
// Copy the master values ...
- Entry(1) = q(1);
- Entry(2) = q(2);
- Entry(3) = q(3);
- Entry(4) = q(4);
+ data[0] = q(1);
+ data[1] = q(2);
+ data[2] = q(3);
+ data[3] = q(4);
+ ComputeDerived();
// .. and copy the derived values if they are valid
mCacheValid = q.mCacheValid;
if (mCacheValid) {
return *this;
}
+ /// Conversion from Quat to Matrix
+ operator FGMatrix33() const { return GetT(); }
+
/** Comparison operator "==".
@param q a quaternion reference
@return true if both quaternions represent the same rotation. */
bool operator==(const FGQuaternion& q) const {
- return Entry(1) == q(1) && Entry(2) == q(2)
- && Entry(3) == q(3) && Entry(4) == q(4);
+ return data[0] == q(1) && data[1] == q(2)
+ && data[2] == q(3) && data[3] == q(4);
}
/** Comparison operator "!=".
bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
const FGQuaternion& operator+=(const FGQuaternion& q) {
// Copy the master values ...
- Entry(1) += q(1);
- Entry(2) += q(2);
- Entry(3) += q(3);
- Entry(4) += q(4);
+ data[0] += q(1);
+ data[1] += q(2);
+ data[2] += q(3);
+ data[3] += q(4);
mCacheValid = false;
return *this;
}
@return a quaternion reference representing Q, where Q = Q - q. */
const FGQuaternion& operator-=(const FGQuaternion& q) {
// Copy the master values ...
- Entry(1) -= q(1);
- Entry(2) -= q(2);
- Entry(3) -= q(3);
- Entry(4) -= q(4);
+ data[0] -= q(1);
+ data[1] -= q(2);
+ data[2] -= q(3);
+ data[3] -= q(4);
mCacheValid = false;
return *this;
}
@param scalar a multiplicative value.
@return a quaternion reference representing Q, where Q = Q * scalar. */
const FGQuaternion& operator*=(double scalar) {
- Entry(1) *= scalar;
- Entry(2) *= scalar;
- Entry(3) *= scalar;
- Entry(4) *= scalar;
+ data[0] *= scalar;
+ data[1] *= scalar;
+ data[2] *= scalar;
+ data[3] *= scalar;
mCacheValid = false;
return *this;
}
@param q a quaternion to be summed.
@return a quaternion representing Q, where Q = Q + q. */
FGQuaternion operator+(const FGQuaternion& q) const {
- return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
- Entry(3)+q(3), Entry(4)+q(4));
+ return FGQuaternion(data[0]+q(1), data[1]+q(2),
+ data[2]+q(3), data[3]+q(4));
}
/** Arithmetic operator "-".
@param q a quaternion to be subtracted.
@return a quaternion representing Q, where Q = Q - q. */
FGQuaternion operator-(const FGQuaternion& q) const {
- return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
- Entry(3)-q(3), Entry(4)-q(4));
+ return FGQuaternion(data[0]-q(1), data[1]-q(2),
+ data[2]-q(3), data[3]-q(4));
}
/** Arithmetic operator "*".
@param q a quaternion to be multiplied.
@return a quaternion representing Q, where Q = Q * q. */
FGQuaternion operator*(const FGQuaternion& q) const {
- return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
- Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
- Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
- Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
+ return FGQuaternion(data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4),
+ data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3),
+ data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2),
+ data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1));
}
/** Arithmetic operator "*=".
@param q a quaternion to be multiplied.
@return a quaternion reference representing Q, where Q = Q * q. */
const FGQuaternion& operator*=(const FGQuaternion& q) {
- double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
- double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
- double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
- double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
- Entry(1) = q0;
- Entry(2) = q1;
- Entry(3) = q2;
- Entry(4) = q3;
+ double q0 = data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4);
+ double q1 = data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3);
+ double q2 = data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2);
+ double q3 = data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1);
+ data[0] = q0;
+ data[1] = q1;
+ data[2] = q2;
+ data[3] = q3;
mCacheValid = false;
return *this;
}
the identity orientation.
*/
FGQuaternion Inverse(void) const {
- double norm = Magnitude();
+ double norm = SqrMagnitude();
if (norm == 0.0)
return *this;
double rNorm = 1.0/norm;
- return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
- -Entry(3)*rNorm, -Entry(4)*rNorm );
+ return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
+ -data[2]*rNorm, -data[3]*rNorm );
}
/** Conjugate of the quaternion.
to the inverse iff the quaternion is normalized.
*/
FGQuaternion Conjugate(void) const {
- return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
+ return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
}
friend FGQuaternion operator*(double, const FGQuaternion&);
Compute and return the square of the euclidean norm of this vector.
*/
double SqrMagnitude(void) const {
- return Entry(1)*Entry(1)+Entry(2)*Entry(2)
- +Entry(3)*Entry(3)+Entry(4)*Entry(4);
+ return data[0]*data[0] + data[1]*data[1]
+ + data[2]*data[2] + data[3]*data[3];
}
- /** Normialze.
+ /** Normalize.
Normalize the vector to have the Magnitude() == 1.0. If the vector
is equal to zero it is left untouched.
private:
/** Copying by assigning the vector valued components. */
FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
- { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
+ { data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }
/** Computation of derived values.
This function recomputes the derived values like euler angles and
This function checks if the derived values like euler angles and
transformation matrices are already computed. If so, it
returns. If they need to be computed the real worker routine
- \ref FGQuaternion::ComputeDerivedUnconditional(void) const
- is called.
- This function is inlined to avoid function calls in the fast path. */
+ FGQuaternion::ComputeDerivedUnconditional(void) const
+ is called. */
void ComputeDerived(void) const {
if (!mCacheValid)
ComputeDerivedUnconditional();
}
/** The quaternion values itself. This is the master copy. */
- double mData[4];
+ double data[4];
/** A data validity flag.
This class implements caching of the derived values like the
/** The cached sines and cosines of the euler angles. */
mutable FGColumnVector3 mEulerSines;
mutable FGColumnVector3 mEulerCosines;
+
+ void Debug(int from) const;
+
+ void InitializeFromEulerAngles(double phi, double tht, double psi);
};
/** Scalar multiplication.
@param scalar scalar value to multiply with.
- @param p Vector to multiply.
+ @param q Vector to multiply.
Multiply the Vector with a scalar value.
*/
} // namespace JSBSim
+#include "FGMatrix33.h"
+
#endif
projects / flightgear.git / blobdiff