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"
 
namespace JSBSim {
 
class FGMatrix33;
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  CLASS DOCUMENTATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  CLASS DECLARATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
class JSBSIM_API 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;
}
 
JSBSIM_API std::ostream& operator<<(std::ostream& os, const FGQuaternion& q);
 
} // namespace JSBSim
#endif