fgquaternion.h

6 DOF Missle Simulation

  /** 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 mData[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 ...
    Entry(1) = q(1);
    Entry(2) = q(2);
    Entry(3) = q(3);
    Entry(4) = q(4);
    // .. 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;
  }

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

  /** 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 ...
    Entry(1) += q(1);
    Entry(2) += q(2);
    Entry(3) += q(3);
    Entry(4) += q(4);
    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 ...
    Entry(1) -= q(1);
    Entry(2) -= q(2);
    Entry(3) -= q(3);
    Entry(4) -= q(4);
    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) {
    Entry(1) *= scalar;
    Entry(2) *= scalar;
    Entry(3) *= scalar;
    Entry(4) *= 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(Entry(1)+q(1), Entry(2)+q(2),
                        Entry(3)+q(3), Entry(4)+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));
  }

  /** 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(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));
  }

  /** 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 = 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;
    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 = Magnitude();
    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 );
  }

  /** 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( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
  }

  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 Entry(1)*Entry(1)+Entry(2)*Entry(2)
      +Entry(3)*Entry(3)+Entry(4)*Entry(4);
  }

  /** Normialze.

      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)
    { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = 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
      \ref FGQuaternion::ComputeDerivedUnconditional(void) const
      is called.
      This function is inlined to avoid function calls in the fast path. */
  void ComputeDerived(void) const {
    if (!mCacheValid)
      ComputeDerivedUnconditional();
  }

  /** The quaternion values itself. This is the master copy. */
  double mData[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;
};

/** 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(1), scalar*q(2), scalar*q(3), scalar*q(4));
}

} // namespace JSBSim

#endif