flightgear/src/FDM/JSBSim/math/FGQuaternion.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGQuaternion.h
 Author:       Jon Berndt, Mathis Froehlich
 Date started: 12/02/98

 ------- 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 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 Lesser General Public License for more
 details.

 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 Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.

HISTORY
-------------------------------------------------------------------------------
12/02/98   JSB   Created
15/01/04   MF    Quaternion class from old FGColumnVector4

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGQUATERNION_H
#define FGQUATERNION_H

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  INCLUDES
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGJSBBase.h"
#include "FGColumnVector3.h"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  DEFINITIONS
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.23 2011/10/31 14:54:40 bcoconni Exp $"

namespace JSBSim {

class FGMatrix33;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  CLASS DOCUMENTATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/**  Models the Quaternion representation of rotations.
    FGQuaternion is a representation of an arbitrary rotation through a
    quaternion. It has vector properties. This class also contains access
    functions to the euler angle representation of rotations and access to
    transformation matrices for 3D vectors. Transformations and euler angles are
    therefore computed once they are requested for the first time. Then they are
    cached for later usage as long as the class is not accessed trough
    a nonconst member function.

    Note: The order of rotations used in this class corresponds to a 3-2-1 sequence,
    or Y-P-R, or Z-Y-X, if you prefer.

    @see Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
    Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
    School, January 1994
    @see D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
    JSC 12960, July 1977
    @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
    NASA-Ames", NASA CR-2497, January 1975
    @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
    Wiley & Sons, 1979 ISBN 0-471-03032-5
    @see Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
    1982 ISBN 0-471-08936-2
    @author Mathias Froehlich, extended FGColumnVector4 originally by Tony Peden
            and Jon Berndt
*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  CLASS DECLARATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGQuaternion : public FGJSBBase {
public:
  /** Default initializer.
      Default initializer, initializes the class with the identity rotation.  */
  FGQuaternion() : mCacheValid(false) {
    data[0] = 1.0;
    data[1] = data[2] = data[3] = 0.0;
  }

  /** Copy constructor.
      Copy constructor, initializes the quaternion.
      @param q  a constant reference to another FGQuaternion instance  */
  FGQuaternion(const FGQuaternion& q);

  /** Initializer by euler angles.
      Initialize the quaternion with the euler angles.
      @param phi The euler X axis (roll) angle in radians
      @param tht The euler Y axis (attitude) angle in radians
      @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 idx Index of the euler angle to initialize
      @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) {
      data[0] = Cangle2;
      data[1] = Sangle2;
      data[2] = 0.0;
      data[3] = 0.0;

    } else if (idx == eTht) {
      data[0] = Cangle2;
      data[1] = 0.0;
      data[2] = Sangle2;
      data[3] = 0.0;

    } else {
      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 derivative for given angular rates.
      Computes the quaternion derivative which results from the given
      angular velocities
      @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) const;

  /** Transformation matrix.
      @return a reference to the transformation/rotation matrix
      corresponding to this quaternion rotation.  */
  const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }

  /** Backward transformation matrix.
      @return a reference to the inverse transformation/rotation matrix
      corresponding to this quaternion rotation.  */
  const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }

  /** Retrieves the Euler angles.
      @return a reference to the triad of Euler angles corresponding
      to this quaternion rotation.
      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.
   */
  double GetEuler(int i) const {
    ComputeDerived();
    return mEulerAngles(i);
  }

  /** Retrieves the Euler angles.
      @param i the Euler angle index.
      @return a reference to the i-th euler angles corresponding
      to this quaternion rotation.
      units degrees */
  double GetEulerDeg(int i) const {
    ComputeDerived();
    return radtodeg*mEulerAngles(i);
  }

  /** Retrieves the Euler angle vector.
      @return an Euler angle column vector corresponding
      to this quaternion rotation.
      units degrees */
  FGColumnVector3 const GetEulerDeg(void) const {
    ComputeDerived();
    return radtodeg*mEulerAngles;
  }

  /** Retrieves sine of the given euler angle.
      @return the sine of the Euler angle theta (pitch attitude) corresponding
      to this quaternion rotation.  */
  double GetSinEuler(int i) const {
    ComputeDerived();
    return mEulerSines(i);
  }

  /** Retrieves cosine of the given euler angle.
      @return the sine of the Euler angle theta (pitch attitude) corresponding
      to this quaternion rotation.  */
  double GetCosEuler(int i) const {
    ComputeDerived();
    return mEulerCosines(i);
  }

  /** Read access the entries of the vector.

      @param idx the component index.

      Return the value of the matrix entry at the given index.
      Indices are counted starting with 1.

      Note that the index given in the argument is unchecked.
   */
  double operator()(unsigned int idx) const { return data[idx-1]; }

  /** Write access the entries of the vector.

      @param idx the component index.

      Return a reference to the vector entry at the given index.
      Indices are counted starting with 1.

      Note that the index given in the argument is unchecked.
   */
  double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }

  /** Read access the entries of the vector.

      @param idx the component index.

      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 <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 data[idx-1]; }

  /** Write access the entries of the vector.

      @param idx the component index.

      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 <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 data[idx-1];
  }

  /** Assignment operator "=".
      Assign the value of q to the current object. Cached values are
      conserved.
      @param q reference to an FGQuaternion instance
      @return reference to a quaternion object  */
  const FGQuaternion& operator=(const FGQuaternion& q) {
    // Copy the master values ...
    data[0] = q.data[0];
    data[1] = q.data[1];
    data[2] = q.data[2];
    data[3] = q.data[3];
    ComputeDerived();
    // .. and copy the derived values if they are valid
    mCacheValid = q.mCacheValid;
    if (mCacheValid) {
        mT = q.mT;
        mTInv = q.mTInv;
        mEulerAngles = q.mEulerAngles;
        mEulerSines = q.mEulerSines;
        mEulerCosines = q.mEulerCosines;
    }
    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 data[0] == q.data[0] && data[1] == q.data[1]
      && data[2] == q.data[2] && data[3] == q.data[3];
  }

  /** Comparison operator "!=".
      @param q a quaternion reference
      @return true if both quaternions do not represent the same rotation.  */
  bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
  const FGQuaternion& operator+=(const FGQuaternion& q) {
    // Copy the master values ...
    data[0] += q.data[0];
    data[1] += q.data[1];
    data[2] += q.data[2];
    data[3] += q.data[3];
    mCacheValid = false;
    return *this;
  }

  /** Arithmetic operator "-=".
      @param q a quaternion reference.
      @return a quaternion reference representing Q, where Q = Q - q. */
  const FGQuaternion& operator-=(const FGQuaternion& q) {
    // Copy the master values ...
    data[0] -= q.data[0];
    data[1] -= q.data[1];
    data[2] -= q.data[2];
    data[3] -= q.data[3];
    mCacheValid = false;
    return *this;
  }

  /** Arithmetic operator "*=".
      @param scalar a multiplicative value.
      @return a quaternion reference representing Q, where Q = Q * scalar. */
  const FGQuaternion& operator*=(double scalar) {
    data[0] *= scalar;
    data[1] *= scalar;
    data[2] *= scalar;
    data[3] *= scalar;
    mCacheValid = false;
    return *this;
  }

  /** Arithmetic operator "/=".
      @param scalar a divisor value.
      @return a quaternion reference representing Q, where Q = Q / scalar. */
  const FGQuaternion& operator/=(double scalar) {
    return operator*=(1.0/scalar);
  }

  /** Arithmetic operator "+".
      @param q a quaternion to be summed.
      @return a quaternion representing Q, where Q = Q + q. */
  FGQuaternion operator+(const FGQuaternion& q) const {
    return FGQuaternion(data[0]+q.data[0], data[1]+q.data[1],
                        data[2]+q.data[2], data[3]+q.data[3]);
  }

  /** 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(data[0]-q.data[0], data[1]-q.data[1],
                        data[2]-q.data[2], data[3]-q.data[3]);
  }

  /** Arithmetic operator "*".
      Multiplication of two quaternions is like performing successive rotations.
      @param q a quaternion to be multiplied.
      @return a quaternion representing Q, where Q = Q * q. */
  FGQuaternion operator*(const FGQuaternion& q) const {
    return FGQuaternion(data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3],
                        data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2],
                        data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1],
                        data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0]);
  }

  /** Arithmetic operator "*=".
      Multiplication of two quaternions is like performing successive rotations.
      @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 = data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3];
    double q1 = data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2];
    double q2 = data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1];
    double q3 = data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0];
    data[0] = q0;
    data[1] = q1;
    data[2] = q2;
    data[3] = q3;
    mCacheValid = false;
    return *this;
  }

  /** Inverse of the quaternion.

      Compute and return the inverse of the quaternion so that the orientation
      represented with *this multiplied with the returned value is equal to
      the identity orientation.
  */
  FGQuaternion Inverse(void) const {
    double norm = SqrMagnitude();
    if (norm == 0.0)
      return *this;
    double rNorm = 1.0/norm;
    return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
                         -data[2]*rNorm, -data[3]*rNorm );
  }

  /** Conjugate of the quaternion.

      Compute and return the conjugate of the quaternion. This one is equal
      to the inverse iff the quaternion is normalized.
  */
  FGQuaternion Conjugate(void) const {
    return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
  }

  friend FGQuaternion operator*(double, const FGQuaternion&);

  /** Length of the vector.

      Compute and return the euclidean norm of this vector.
  */
  double Magnitude(void) const { return sqrt(SqrMagnitude()); }

  /** Square of the length of the vector.

      Compute and return the square of the euclidean norm of this vector.
  */
  double SqrMagnitude(void) const {
    return  data[0]*data[0] + data[1]*data[1]
          + data[2]*data[2] + data[3]*data[3];
  }

  /** Normalize.

      Normalize the vector to have the Magnitude() == 1.0. If the vector
      is equal to zero it is left untouched.
   */
  void Normalize(void);

  /** Zero quaternion vector. Does not represent any orientation.
      Useful for initialization of increments */
  static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }

private:
  /** Copying by assigning the vector valued components.  */
  FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
    { 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
      transformation matrices. It does this unconditionally.  */
  void ComputeDerivedUnconditional(void) const;

  /** Computation of derived values.
      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
      FGQuaternion::ComputeDerivedUnconditional(void) const
      is called. */
  void ComputeDerived(void) const {
    if (!mCacheValid)
      ComputeDerivedUnconditional();
  }

  /** The quaternion values itself. This is the master copy. */
  double data[4];

  /** A data validity flag.
      This class implements caching of the derived values like the
      orthogonal rotation matrices or the Euler angles. For caching we
      carry a flag which signals if the values are valid or not.
      The C++ keyword "mutable" tells the compiler that the data member is
      allowed to change during a const member function.  */
  mutable bool mCacheValid;

  /** This stores the transformation matrices.  */
  mutable FGMatrix33 mT;
  mutable FGMatrix33 mTInv;

  /** The cached euler angles.  */
  mutable FGColumnVector3 mEulerAngles;

  /** The cached sines and cosines of the euler angles.  */
  mutable FGColumnVector3 mEulerSines;
  mutable FGColumnVector3 mEulerCosines;

  void InitializeFromEulerAngles(double phi, double tht, double psi);
};

/** Scalar multiplication.

    @param scalar scalar value to multiply with.
    @param q Vector to multiply.

    Multiply the Vector with a scalar value.
*/
inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
  return FGQuaternion(scalar*q.data[0], scalar*q.data[1], scalar*q.data[2], scalar*q.data[3]);
}

/** Write quaternion to a stream.
    @param os Stream to write to.
    @param q Quaternion to write.
    Write the quaternion to a stream.*/
std::ostream& operator<<(std::ostream& os, const FGQuaternion& q);

} // namespace JSBSim
#endif