src/FDM/JSBSim/math/FGMatrix33.h

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