src/FDM/JSBSim/FGMatrix33.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Header: FGMatrix33.h
   4 Author: Tony Peden, Jon Berndt, Mathias Frolich
   5 Date started: Unknown
   6
   7 HISTORY
   8 --------------------------------------------------------------------------------
   9 ??/??/??   TP   Created
  10 03/16/2000 JSB  Added exception throwing
  11 03/06/2004 MF   Rework of the code to make it a bit compiler friendlier
  12
  13 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  14 SENTRY
  15 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  16
  17 #ifndef FGMATRIX33_H
  18 #define FGMATRIX33_H
  19
  20 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  21 INCLUDES
  22 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  23
  24 #include <stdlib.h>
  25 #ifdef FGFS
  26 #  include <math.h>
  27 #  include <simgear/compiler.h>
  28 #  include STL_STRING
  29 #  include STL_FSTREAM
  30 #  include STL_IOSTREAM
  31    SG_USING_STD(string);
  32    SG_USING_STD(ostream);
  33    SG_USING_STD(istream);
  34    SG_USING_STD(cerr);
  35    SG_USING_STD(cout);
  36    SG_USING_STD(endl);
  37 #else
  38 #  include <string>
  39 #  if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
  40      include <fstream.h>
  41      include <iostream.h>
  42 #    include <math.h>
  43 #  else
  44 #    include <fstream>
  45 #    include <iostream>
  46 #    if defined(sgi) && !defined(__GNUC__)
  47 #      include <math.h>
  48 #    else
  49 #      include <cmath>
  50 #    endif
  51      using std::ostream;
  52      using std::istream;
  53      using std::cerr;
  54      using std::cout;
  55      using std::endl;
  56 #  endif
  57    using std::string;
  58 #endif
  59
  60 #include "FGColumnVector3.h"
  61 #include "FGJSBBase.h"
  62
  63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  64 DEFINITIONS
  65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  66
  67 #define ID_MATRIX33 "$Id$"
  68
  69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70 FORWARD DECLARATIONS
  71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  72
  73 namespace JSBSim {
  74
  75 class FGColumnVector3;
  76
  77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78 CLASS DOCUMENTATION
  79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  80
  81 /** Exception convenience class.
  82   */
  83
  84 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  85 DECLARATION: MatrixException
  86 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  87
  88 class MatrixException : public FGJSBBase
  89 {
  90 public:
  91   string Message;
  92 };
  93
  94 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  95 CLASS DOCUMENTATION
  96 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  97
  98   /** Handles matrix math operations.
  99       @author Tony Peden, Jon Berndt, Mathias Froelich
 100   */
 101
 102 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 103 DECLARATION: FGMatrix33
 104 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 105
 106 class FGMatrix33 : public FGJSBBase
 107 {
 108 public:
 109
 110   enum {
 111     eRows = 3,
 112     eColumns = 3
 113   };
 114
 115   /** Default initializer.
 116       Create a zero matrix.  */
 117   FGMatrix33(void);
 118
 119   /** Copy constructor.
 120       @param M Matrix which is used for initialization.
 121       Create copy of the matrix given in the argument.  */
 122   FGMatrix33(const FGMatrix33& M) {
 123     Entry(1,1) = M.Entry(1,1);
 124     Entry(2,1) = M.Entry(2,1);
 125     Entry(3,1) = M.Entry(3,1);
 126     Entry(1,2) = M.Entry(1,2);
 127     Entry(2,2) = M.Entry(2,2);
 128     Entry(3,2) = M.Entry(3,2);
 129     Entry(1,3) = M.Entry(1,3);
 130     Entry(2,3) = M.Entry(2,3);
 131     Entry(3,3) = M.Entry(3,3);
 132
 133     Debug(0);
 134   }
 135
 136   /** Initialization by given values.
 137       @param m11 value of the 1,1 Matrix element.
 138       @param m12 value of the 1,2 Matrix element.
 139       @param m13 value of the 1,3 Matrix element.
 140       @param m21 value of the 2,1 Matrix element.
 141       @param m22 value of the 2,2 Matrix element.
 142       @param m23 value of the 2,3 Matrix element.
 143       @param m31 value of the 3,1 Matrix element.
 144       @param m32 value of the 3,2 Matrix element.
 145       @param m33 value of the 3,3 Matrix element.
 146       Create a matrix from the doubles given in the arguments.   */
 147   FGMatrix33(double m11, double m12, double m13,
 148              double m21, double m22, double m23,
 149              double m31, double m32, double m33) {
 150     Entry(1,1) = m11;
 151     Entry(2,1) = m21;
 152     Entry(3,1) = m31;
 153     Entry(1,2) = m12;
 154     Entry(2,2) = m22;
 155     Entry(3,2) = m32;
 156     Entry(1,3) = m13;
 157     Entry(2,3) = m23;
 158     Entry(3,3) = m33;
 159
 160     Debug(0);
 161   }
 162
 163   /// Destructor.
 164   ~FGMatrix33(void) { Debug(1); }
 165
 166   /** Read access the entries of the matrix.
 167       @param row Row index.
 168       @param col Column index.
 169       @return the value of the matrix entry at the given row and
 170       column indices. Indices are counted starting with 1.   */
 171   double operator()(unsigned int row, unsigned int col) const {
 172     return Entry(row, col);
 173   }
 174
 175   /** Write access the entries of the matrix.
 176       Note that the indices given in the arguments are unchecked.
 177       @param row Row index.
 178       @param col Column index.
 179       @return a reference to the matrix entry at the given row and
 180       column indices. Indices are counted starting with 1.   */
 181   double& operator()(unsigned int row, unsigned int col) {
 182     return Entry(row, col);
 183   }
 184
 185   /** Read access the entries of the matrix.
 186       This function is just a shortcut for the @ref double&
 187       operator()(unsigned int row, unsigned int col) function. It is
 188       used internally to access the elements in a more convenient way.
 189       Note that the indices given in the arguments are unchecked.
 190       @param row Row index.
 191       @param col Column index.
 192       @return the value of the matrix entry at the given row and
 193       column indices. Indices are counted starting with 1.   */
 194   double Entry(unsigned int row, unsigned int col) const {
 195     return data[(col-1)*eRows+row-1];
 196   }
 197
 198   /** Write access the entries of the matrix.
 199       This function is just a shortcut for the @ref double&
 200       operator()(unsigned int row, unsigned int col) function. It is
 201       used internally to access the elements in a more convenient way.
 202       Note that the indices given in the arguments are unchecked.
 203       @param row Row index.
 204       @param col Column index.
 205       @return a reference to the matrix entry at the given row and
 206       column indices. Indices are counted starting with 1.   */
 207    double& Entry(unsigned int row, unsigned int col) {
 208      return data[(col-1)*eRows+row-1];
 209    }
 210
 211   /** Number of rows in the matrix.
 212       @return the number of rows in the matrix.   */
 213    unsigned int Rows(void) const { return eRows; }
 214
 215   /** Number of cloumns in the matrix.
 216       @return the number of columns in the matrix.   */
 217    unsigned int Cols(void) const { return eColumns; }
 218
 219   /** Transposed matrix.
 220       This function only returns the transpose of this matrix. This matrix itself
 221       remains unchanged.
 222       @return the transposed matrix.   */
 223   FGMatrix33 Transposed(void) const {
 224     return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
 225                        Entry(1,2), Entry(2,2), Entry(3,2),
 226                        Entry(1,3), Entry(2,3), Entry(3,3) );
 227   }
 228
 229   /** Transposes this matrix.
 230       This function only transposes this matrix. Nothing is returned.   */
 231   void T(void);
 232
 233 /** Initialize the matrix.
 234     This function initializes a matrix to all 0.0. */
 235   void InitMatrix(void);
 236
 237 /** Initialize the matrix.
 238     This function initializes a matrix to user specified values. */
 239   void InitMatrix(double m11, double m12, double m13,
 240                   double m21, double m22, double m23,
 241                   double m31, double m32, double m33) {
 242     Entry(1,1) = m11;
 243     Entry(2,1) = m21;
 244     Entry(3,1) = m31;
 245     Entry(1,2) = m12;
 246     Entry(2,2) = m22;
 247     Entry(3,2) = m32;
 248     Entry(1,3) = m13;
 249     Entry(2,3) = m23;
 250     Entry(3,3) = m33;
 251   }
 252
 253   /** Determinant of the matrix.
 254       @return the determinant of the matrix.   */
 255   double Determinant(void) const;
 256
 257   /** Return if the matrix is invertible.
 258       Checks and returns if the matrix is nonsingular and thus
 259       invertible. This is done by simply computing the determinant and
 260       check if it is zero. Note that this test does not cover any
 261       instabilities caused by nearly singular matirces using finite
 262       arithmetics. It only checks exact singularity.   */
 263   bool Invertible(void) const { return 0.0 != Determinant(); }
 264
 265   /** Return the inverse of the matrix.
 266       Computes and returns if the inverse of the matrix. It is computed
 267       by Cramers Rule. Also there are no checks performed if the matrix
 268       is invertible. If you are not sure that it really is check this
 269       with the @ref Invertible() call before.   */
 270   FGMatrix33 Inverse(void) const;
 271
 272   /** Assignment operator.
 273       @param A source matrix.
 274       Copy the content of the matrix given in the argument into *this.   */
 275   FGMatrix33& operator=(const FGMatrix33& A) {
 276     data[0] = A.data[0];
 277     data[1] = A.data[1];
 278     data[2] = A.data[2];
 279     data[3] = A.data[3];
 280     data[4] = A.data[4];
 281     data[5] = A.data[5];
 282     data[6] = A.data[6];
 283     data[7] = A.data[7];
 284     data[8] = A.data[8];
 285     return *this;
 286   }
 287
 288   /** Matrix vector multiplication.
 289       @param v vector to multiply with.
 290       @return matric vector product.
 291       Compute and return the product of the current matrix with the
 292       vector given in the argument.   */
 293   FGColumnVector3 operator*(const FGColumnVector3& v) const;
 294
 295   /** Matrix subtraction.
 296       @param B matrix to add to.
 297       @return difference of the matrices.
 298       Compute and return the sum of the current matrix and the matrix
 299       B given in the argument.  */
 300   FGMatrix33 operator-(const FGMatrix33& B) const;
 301
 302   /** Matrix addition.
 303       @param B matrix to add to.
 304       @return sum of the matrices.
 305       Compute and return the sum of the current matrix and the matrix
 306       B given in the argument.  */
 307   FGMatrix33 operator+(const FGMatrix33& B) const;
 308
 309   /** Matrix product.
 310       @param B matrix to add to.
 311       @return product of the matrices.
 312       Compute and return the product of the current matrix and the matrix
 313       B given in the argument.  */
 314   FGMatrix33 operator*(const FGMatrix33& B) const;
 315
 316   /** Multiply the matrix with a scalar.
 317       @param scalar scalar factor to multiply with.
 318       @return scaled matrix.
 319       Compute and return the product of the current matrix with the
 320       scalar value scalar given in the argument.  */
 321   FGMatrix33 operator*(const double scalar) const;
 322
 323   /** Multiply the matrix with 1.0/scalar.
 324       @param scalar scalar factor to divide through.
 325       @return scaled matrix.
 326       Compute and return the product of the current matrix with the
 327       scalar value 1.0/scalar, where scalar is given in the argument.  */
 328   FGMatrix33 operator/(const double scalar) const;
 329
 330   /** In place matrix subtraction.
 331       @param B matrix to subtract.
 332       @return reference to the current matrix.
 333       Compute the diffence from the current matrix and the matrix B
 334       given in the argument.  */
 335   FGMatrix33& operator-=(const FGMatrix33 &B);
 336
 337   /** In place matrix addition.
 338       @param B matrix to add.
 339       @return reference to the current matrix.
 340       Compute the sum of the current matrix and the matrix B
 341       given in the argument.  */
 342   FGMatrix33& operator+=(const FGMatrix33 &B);
 343
 344   /** In place matrix multiplication.
 345       @param B matrix to multiply with.
 346       @return reference to the current matrix.
 347       Compute the product of the current matrix and the matrix B
 348       given in the argument.  */
 349   FGMatrix33& operator*=(const FGMatrix33 &B);
 350
 351   /** In place matrix scale.
 352       @param scalar scalar value to multiply with.
 353       @return reference to the current matrix.
 354       Compute the product of the current matrix and the scalar value scalar
 355       given in the argument.  */
 356   FGMatrix33& operator*=(const double scalar);
 357
 358   /** In place matrix scale.
 359       @param scalar scalar value to divide through.
 360       @return reference to the current matrix.
 361       Compute the product of the current matrix and the scalar value
 362       1.0/scalar, where scalar is given in the argument.  */
 363   FGMatrix33& operator/=(const double scalar);
 364
 365 private:
 366   double data[eRows*eColumns];
 367
 368   void Debug(int from);
 369 };
 370
 371 /** Scalar multiplication.
 372     @param scalar scalar value to multiply with.
 373     @param A Matrix to multiply.
 374     Multiply the Matrix with a scalar value.*/
 375 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
 376   // use already defined operation.
 377   return A*scalar;
 378 }
 379
 380 /** Write matrix to a stream.
 381     @param os Stream to write to.
 382     @param M Matrix to write.
 383     Write the matrix to a stream.*/
 384 ostream& operator<<(ostream& os, const FGMatrix33& M);
 385
 386 /** Read matrix from a stream.
 387     @param os Stream to read from.
 388     @param M Matrix to initialize with the values from the stream.
 389     Read matrix from a stream.*/
 390 istream& operator>>(istream& is, FGMatrix33& M);
 391
 392 } // namespace JSBSim
 393
 394 #endif