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
 117       Create a zero matrix.
 118    */
 119   FGMatrix33(void);
 120
 121   /** Copy constructor.
 122
 123       @param M Matrix which is used for initialization.
 124
 125       Create copy of the matrix given in the argument.
 126    */
 127   FGMatrix33(const FGMatrix33& M) {
 128     Entry(1,1) = M.Entry(1,1);
 129     Entry(2,1) = M.Entry(2,1);
 130     Entry(3,1) = M.Entry(3,1);
 131     Entry(1,2) = M.Entry(1,2);
 132     Entry(2,2) = M.Entry(2,2);
 133     Entry(3,2) = M.Entry(3,2);
 134     Entry(1,3) = M.Entry(1,3);
 135     Entry(2,3) = M.Entry(2,3);
 136     Entry(3,3) = M.Entry(3,3);
 137
 138     Debug(0);
 139   }
 140
 141   /** Initialization by given values.
 142
 143       @param m11 value of the 1,1 Matrix element.
 144       @param m12 value of the 1,2 Matrix element.
 145       @param m13 value of the 1,3 Matrix element.
 146       @param m21 value of the 2,1 Matrix element.
 147       @param m22 value of the 2,2 Matrix element.
 148       @param m23 value of the 2,3 Matrix element.
 149       @param m31 value of the 3,1 Matrix element.
 150       @param m32 value of the 3,2 Matrix element.
 151       @param m33 value of the 3,3 Matrix element.
 152
 153       Create a matrix from the doubles given in the arguments.
 154    */
 155   FGMatrix33(double m11, double m12, double m13,
 156              double m21, double m22, double m23,
 157              double m31, double m32, double m33) {
 158     Entry(1,1) = m11;
 159     Entry(2,1) = m21;
 160     Entry(3,1) = m31;
 161     Entry(1,2) = m12;
 162     Entry(2,2) = m22;
 163     Entry(3,2) = m32;
 164     Entry(1,3) = m13;
 165     Entry(2,3) = m23;
 166     Entry(3,3) = m33;
 167
 168     Debug(0);
 169   }
 170
 171   /** Destructor.
 172    */
 173   ~FGMatrix33(void) { Debug(1); }
 174
 175   /** Read access the entries of the matrix.
 176       @param row Row index.
 177       @param col Column index.
 178
 179       @return the value of the matrix entry at the given row and
 180       column indices. Indices are counted starting with 1.
 181    */
 182   double operator()(unsigned int row, unsigned int col) const {
 183     return Entry(row, col);
 184   }
 185
 186   /** Write access the entries of the matrix.
 187       Note that the indices given in the arguments are unchecked.
 188
 189       @param row Row index.
 190       @param col Column index.
 191
 192       @return a reference to the matrix entry at the given row and
 193       column indices. Indices are counted starting with 1.
 194    */
 195   double& operator()(unsigned int row, unsigned int col) {
 196     return Entry(row, col);
 197   }
 198
 199   /** Read access the entries of the matrix.
 200       This function is just a shortcut for the @ref double&
 201       operator()(unsigned int row, unsigned int col) function. It is
 202       used internally to access the elements in a more convenient way.
 203
 204       Note that the indices given in the arguments are unchecked.
 205
 206       @param row Row index.
 207       @param col Column index.
 208
 209       @return the value of the matrix entry at the given row and
 210       column indices. Indices are counted starting with 1.
 211    */
 212   double Entry(unsigned int row, unsigned int col) const {
 213     return data[(col-1)*eRows+row-1];
 214   }
 215
 216   /** Write access the entries of the matrix.
 217       This function is just a shortcut for the @ref double&
 218       operator()(unsigned int row, unsigned int col) function. It is
 219       used internally to access the elements in a more convenient way.
 220
 221       Note that the indices given in the arguments are unchecked.
 222
 223       @param row Row index.
 224       @param col Column index.
 225
 226       @return a reference to the matrix entry at the given row and
 227       column indices. Indices are counted starting with 1.
 228    */
 229    double& Entry(unsigned int row, unsigned int col) {
 230      return data[(col-1)*eRows+row-1];
 231    }
 232
 233   /** Number of rows in the matrix.
 234       @return the number of rows in the matrix.
 235    */
 236    unsigned int Rows(void) const { return eRows; }
 237
 238   /** Number of cloumns in the matrix.
 239       @return the number of columns in the matrix.
 240    */
 241    unsigned int Cols(void) const { return eColumns; }
 242
 243   /** Transposed matrix.
 244       This function only returns the transpose of this matrix. This matrix itself
 245       remains unchanged.
 246       @return the transposed matrix.
 247    */
 248   FGMatrix33 Transposed(void) const {
 249     return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
 250                        Entry(1,2), Entry(2,2), Entry(3,2),
 251                        Entry(1,3), Entry(2,3), Entry(3,3) );
 252   }
 253
 254   /** Transposes this matrix.
 255       This function only transposes this matrix. Nothing is returned.
 256    */
 257   void T(void);
 258
 259 /** Initialize the matrix.
 260     This function initializes a matrix to all 0.0.
 261  */
 262   void InitMatrix(void);
 263
 264 /** Initialize the matrix.
 265     This function initializes a matrix to user specified values.
 266  */
 267   void InitMatrix(double m11, double m12, double m13,
 268                   double m21, double m22, double m23,
 269                   double m31, double m32, double m33) {
 270     Entry(1,1) = m11;
 271     Entry(2,1) = m21;
 272     Entry(3,1) = m31;
 273     Entry(1,2) = m12;
 274     Entry(2,2) = m22;
 275     Entry(3,2) = m32;
 276     Entry(1,3) = m13;
 277     Entry(2,3) = m23;
 278     Entry(3,3) = m33;
 279   }
 280
 281   /** Determinant of the matrix.
 282       @return the determinant of the matrix.
 283    */
 284   double Determinant(void) const;
 285
 286   /** Return if the matrix is invertible.
 287       Checks and returns if the matrix is nonsingular and thus
 288       invertible. This is done by simply computing the determinant and
 289       check if it is zero. Note that this test does not cover any
 290       instabilities caused by nearly singular matirces using finite
 291       arithmetics. It only checks exact singularity.
 292    */
 293   bool Invertible(void) const { return 0.0 != Determinant(); }
 294
 295   /** Return the inverse of the matrix.
 296       Computes and returns if the inverse of the matrix. It is computed
 297       by Cramers Rule. Also there are no checks performed if the matrix
 298       is invertible. If you are not sure that it really is check this
 299       with the @ref Invertible() call before.
 300    */
 301   FGMatrix33 Inverse(void) const;
 302
 303   /** Assignment operator.
 304
 305       @param A source matrix.
 306
 307       Copy the content of the matrix given in the argument into *this.
 308    */
 309   FGMatrix33& operator=(const FGMatrix33& A) {
 310     data[0] = A.data[0];
 311     data[1] = A.data[1];
 312     data[2] = A.data[2];
 313     data[3] = A.data[3];
 314     data[4] = A.data[4];
 315     data[5] = A.data[5];
 316     data[6] = A.data[6];
 317     data[7] = A.data[7];
 318     data[8] = A.data[8];
 319     return *this;
 320   }
 321
 322   /** Matrix vector multiplication.
 323
 324       @param v vector to multiply with.
 325       @return matric vector product.
 326
 327       Compute and return the product of the current matrix with the
 328       vector given in the argument.
 329    */
 330   FGColumnVector3 operator*(const FGColumnVector3& v) const;
 331
 332   /** Matrix subtraction.
 333
 334       @param B matrix to add to.
 335       @return difference of the matrices.
 336
 337       Compute and return the sum of the current matrix and the matrix
 338       B given in the argument.
 339   */
 340   FGMatrix33 operator-(const FGMatrix33& B) const;
 341
 342   /** Matrix addition.
 343
 344       @param B matrix to add to.
 345       @return sum of the matrices.
 346
 347       Compute and return the sum of the current matrix and the matrix
 348       B given in the argument.
 349   */
 350   FGMatrix33 operator+(const FGMatrix33& B) const;
 351
 352   /** Matrix product.
 353
 354       @param B matrix to add to.
 355       @return product of the matrices.
 356
 357       Compute and return the product of the current matrix and the matrix
 358       B given in the argument.
 359   */
 360   FGMatrix33 operator*(const FGMatrix33& B) const;
 361
 362   /** Multiply the matrix with a scalar.
 363
 364       @param scalar scalar factor to multiply with.
 365       @return scaled matrix.
 366
 367       Compute and return the product of the current matrix with the
 368       scalar value scalar given in the argument.
 369   */
 370   FGMatrix33 operator*(const double scalar) const;
 371
 372   /** Multiply the matrix with 1.0/scalar.
 373
 374       @param scalar scalar factor to divide through.
 375       @return scaled matrix.
 376
 377       Compute and return the product of the current matrix with the
 378       scalar value 1.0/scalar, where scalar is given in the argument.
 379   */
 380   FGMatrix33 operator/(const double scalar) const;
 381
 382   /** In place matrix subtraction.
 383
 384       @param B matrix to subtract.
 385       @return reference to the current matrix.
 386
 387       Compute the diffence from the current matrix and the matrix B
 388       given in the argument.
 389   */
 390   FGMatrix33& operator-=(const FGMatrix33 &B);
 391
 392   /** In place matrix addition.
 393
 394       @param B matrix to add.
 395       @return reference to the current matrix.
 396
 397       Compute the sum of the current matrix and the matrix B
 398       given in the argument.
 399   */
 400   FGMatrix33& operator+=(const FGMatrix33 &B);
 401
 402   /** In place matrix multiplication.
 403
 404       @param B matrix to multiply with.
 405       @return reference to the current matrix.
 406
 407       Compute the product of the current matrix and the matrix B
 408       given in the argument.
 409   */
 410   FGMatrix33& operator*=(const FGMatrix33 &B);
 411
 412   /** In place matrix scale.
 413
 414       @param scalar scalar value to multiply with.
 415       @return reference to the current matrix.
 416
 417       Compute the product of the current matrix and the scalar value scalar
 418       given in the argument.
 419   */
 420   FGMatrix33& operator*=(const double scalar);
 421
 422   /** In place matrix scale.
 423
 424       @param scalar scalar value to divide through.
 425       @return reference to the current matrix.
 426
 427       Compute the product of the current matrix and the scalar value
 428       1.0/scalar, where scalar is given in the argument.
 429   */
 430   FGMatrix33& operator/=(const double scalar);
 431
 432 private:
 433   double data[eRows*eColumns];
 434
 435   void Debug(int from);
 436 };
 437
 438 /** Scalar multiplication.
 439
 440     @param scalar scalar value to multiply with.
 441     @param A Matrix to multiply.
 442
 443     Multiply the Matrix with a scalar value.
 444 */
 445 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
 446   // use already defined operation.
 447   return A*scalar;
 448 }
 449
 450 /** Write matrix to a stream.
 451
 452     @param os Stream to write to.
 453     @param M Matrix to write.
 454
 455     Write the matrix to a stream.
 456 */
 457 ostream& operator<<(ostream& os, const FGMatrix33& M);
 458
 459 /** Read matrix from a stream.
 460
 461     @param os Stream to read from.
 462     @param M Matrix to initialize with the values from the stream.
 463
 464     Read matrix from a stream.
 465 */
 466 istream& operator>>(istream& is, FGMatrix33& M);
 467
 468 } // namespace JSBSim
 469
 470 #endif