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 <string>
#include "FGJSBBase.h"
#include "FGColumnVector3.h"

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

#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.26 2013/11/24 16:53:15 bcoconni Exp $"

namespace JSBSim {

class FGMatrix33;

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

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

class FGQuaternion : public FGJSBBase {
public:
  FGQuaternion() : mCacheValid(false) {
    data[0] = 1.0;
    data[1] = data[2] = data[3] = 0.0;
  }

  FGQuaternion(const FGQuaternion& q);

  FGQuaternion(double phi, double tht, double psi);

  FGQuaternion(FGColumnVector3 vOrient);

  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;

    }
  }

  FGQuaternion(double angle, const FGColumnVector3& axis)
    : mCacheValid(false) {

    double angle2 = 0.5 * angle;

    double length = axis.Magnitude();
    double Sangle2 = sin(angle2) / length;
    double Cangle2 = cos(angle2);

    data[0] = Cangle2;
    data[1] = Sangle2 * axis(1);
    data[2] = Sangle2 * axis(2);
    data[3] = Sangle2 * axis(3);
  }

  FGQuaternion(const FGMatrix33& m);

  ~FGQuaternion() {}

  FGQuaternion GetQDot(const FGColumnVector3& PQR) const;

  const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }

  const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }

  const FGColumnVector3& GetEuler(void) const {
    ComputeDerived();
    return mEulerAngles;
  }

  double GetEuler(int i) const {
    ComputeDerived();
    return mEulerAngles(i);
  }

  double GetEulerDeg(int i) const {
    ComputeDerived();
    return radtodeg*mEulerAngles(i);
  }

  FGColumnVector3 const GetEulerDeg(void) const {
    ComputeDerived();
    return radtodeg*mEulerAngles;
  }

  double GetSinEuler(int i) const {
    ComputeDerived();
    return mEulerSines(i);
  }

  double GetCosEuler(int i) const {
    ComputeDerived();
    return mEulerCosines(i);
  }

  double operator()(unsigned int idx) const { return data[idx-1]; }

  double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }

  double Entry(unsigned int idx) const { return data[idx-1]; }

  double& Entry(unsigned int idx) {
    mCacheValid = false;
   return data[idx-1];
  }

  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;
  }

  operator FGMatrix33() const { return GetT(); }

  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];
  }

  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;
  }

  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;
  }

  const FGQuaternion& operator*=(double scalar) {
    data[0] *= scalar;
    data[1] *= scalar;
    data[2] *= scalar;
    data[3] *= scalar;
    mCacheValid = false;
    return *this;
  }

  const FGQuaternion& operator/=(double scalar) {
    return operator*=(1.0/scalar);
  }

  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]);
  }

  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]);
  }

  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]);
  }

  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;
  }

  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 );
  }

  FGQuaternion Conjugate(void) const {
    return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
  }

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

  double Magnitude(void) const { return sqrt(SqrMagnitude()); }

  double SqrMagnitude(void) const {
    return  data[0]*data[0] + data[1]*data[1]
          + data[2]*data[2] + data[3]*data[3];
  }

  void Normalize(void);

  static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }

  std::string Dump(const std::string& delimiter) const;

  friend FGQuaternion QExp(const FGColumnVector3& omega);

private:
  FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
    { data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }

  void ComputeDerivedUnconditional(void) const;

  void ComputeDerived(void) const {
    if (!mCacheValid)
      ComputeDerivedUnconditional();
  }

  double data[4];

  mutable bool mCacheValid;

  mutable FGMatrix33 mT;
  mutable FGMatrix33 mTInv;

  mutable FGColumnVector3 mEulerAngles;

  mutable FGColumnVector3 mEulerSines;
  mutable FGColumnVector3 mEulerCosines;

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

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]);
}

inline FGQuaternion QExp(const FGColumnVector3& omega) {
  FGQuaternion qexp;
  double angle = omega.Magnitude();
  double sina_a = angle > 0.0 ? sin(angle)/angle : 1.0;

  qexp.data[0] = cos(angle);
  qexp.data[1] = omega(1) * sina_a;
  qexp.data[2] = omega(2) * sina_a;
  qexp.data[3] = omega(3) * sina_a;

  return qexp;
}

std::ostream& operator<<(std::ostream& os, const FGQuaternion& q);

} // namespace JSBSim
#endif