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 (jon@jsbsim.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 <string>
  44 #include <iosfwd>
  45
  46 #include "FGColumnVector3.h"
  47 #include "FGJSBBase.h"
  48
  49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  50 DEFINITIONS
  51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  52
  53 #define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.12 2010/08/21 17:13:47 jberndt Exp $"
  54
  55 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56 FORWARD DECLARATIONS
  57 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  58
  59 namespace JSBSim {
  60
  61 class FGColumnVector3;
  62 class FGQuaternion;
  63
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  65 CLASS DOCUMENTATION
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  67
  68 /** Exception convenience class.
  69   */
  70
  71 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  72 DECLARATION: MatrixException
  73 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  74
  75 class MatrixException : public FGJSBBase
  76 {
  77 public:
  78   std::string Message;
  79 };
  80
  81 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82 CLASS DOCUMENTATION
  83 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  84
  85   /** Handles matrix math operations.
  86       @author Tony Peden, Jon Berndt, Mathias Froelich
  87   */
  88
  89 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  90 DECLARATION: FGMatrix33
  91 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  92
  93 class FGMatrix33 : public FGJSBBase
  94 {
  95 public:
  96
  97   enum {
  98     eRows = 3,
  99     eColumns = 3
 100   };
 101
 102   /** Default initializer.
 103
 104       Create a zero matrix.
 105    */
 106   FGMatrix33(void);
 107
 108   /** Copy constructor.
 109
 110       @param M Matrix which is used for initialization.
 111
 112       Create copy of the matrix given in the argument.
 113    */
 114   FGMatrix33(const FGMatrix33& M)
 115   {
 116     data[0] = M.data[0];
 117     data[1] = M.data[1];
 118     data[2] = M.data[2];
 119     data[3] = M.data[3];
 120     data[4] = M.data[4];
 121     data[5] = M.data[5];
 122     data[6] = M.data[6];
 123     data[7] = M.data[7];
 124     data[8] = M.data[8];
 125
 126     // Debug(0);
 127   }
 128
 129   /** Initialization by given values.
 130
 131       @param m11 value of the 1,1 Matrix element.
 132       @param m12 value of the 1,2 Matrix element.
 133       @param m13 value of the 1,3 Matrix element.
 134       @param m21 value of the 2,1 Matrix element.
 135       @param m22 value of the 2,2 Matrix element.
 136       @param m23 value of the 2,3 Matrix element.
 137       @param m31 value of the 3,1 Matrix element.
 138       @param m32 value of the 3,2 Matrix element.
 139       @param m33 value of the 3,3 Matrix element.
 140
 141       Create a matrix from the doubles given in the arguments.
 142    */
 143   FGMatrix33(double m11, double m12, double m13,
 144              double m21, double m22, double m23,
 145              double m31, double m32, double m33) {
 146     data[0] = m11;
 147     data[1] = m21;
 148     data[2] = m31;
 149     data[3] = m12;
 150     data[4] = m22;
 151     data[5] = m32;
 152     data[6] = m13;
 153     data[7] = m23;
 154     data[8] = m33;
 155
 156     // Debug(0);
 157   }
 158
 159   /** Destructor.
 160    */
 161   ~FGMatrix33(void) { /* Debug(1); */ }
 162
 163   /** Prints the contents of the matrix.
 164       @param delimeter the item separator (tab or comma)
 165       @return a string with the delimeter-separated contents of the matrix  */
 166   std::string Dump(const std::string& delimeter) const;
 167
 168   /** Prints the contents of the matrix.
 169       @param delimeter the item separator (tab or comma, etc.)
 170       @param prefix an additional prefix that is used to indent the 3X3 matrix printout
 171       @return a string with the delimeter-separated contents of the matrix  */
 172   std::string Dump(const std::string& delimiter, const std::string& prefix) const;
 173
 174   /** Read access the entries of the matrix.
 175       @param row Row index.
 176       @param col Column index.
 177
 178       @return the value of the matrix entry at the given row and
 179       column indices. Indices are counted starting with 1.
 180    */
 181   double operator()(unsigned int row, unsigned int col) const {
 182     return data[(col-1)*eRows+row-1];
 183   }
 184
 185   /** Write access the entries of the matrix.
 186       Note that the indices given in the arguments are unchecked.
 187
 188       @param row Row index.
 189       @param col Column index.
 190
 191       @return a reference to the matrix entry at the given row and
 192       column indices. Indices are counted starting with 1.
 193    */
 194   double& operator()(unsigned int row, unsigned int col) {
 195     return data[(col-1)*eRows+row-1];
 196   }
 197
 198   /** Read access the entries of the matrix.
 199       This function is just a shortcut for the <tt>double&
 200       operator()(unsigned int row, unsigned int col)</tt> function. It is
 201       used internally to access the elements in a more convenient way.
 202
 203       Note that the indices given in the arguments are unchecked.
 204
 205       @param row Row index.
 206       @param col Column index.
 207
 208       @return the value of the matrix entry at the given row and
 209       column indices. Indices are counted starting with 1.
 210    */
 211   double Entry(unsigned int row, unsigned int col) const {
 212     return data[(col-1)*eRows+row-1];
 213   }
 214
 215   /** Write access the entries of the matrix.
 216       This function is just a shortcut for the <tt>double&
 217       operator()(unsigned int row, unsigned int col)</tt> function. It is
 218       used internally to access the elements in a more convenient way.
 219
 220       Note that the indices given in the arguments are unchecked.
 221
 222       @param row Row index.
 223       @param col Column index.
 224
 225       @return a reference to the matrix entry at the given row and
 226       column indices. Indices are counted starting with 1.
 227    */
 228    double& Entry(unsigned int row, unsigned int col) {
 229      return data[(col-1)*eRows+row-1];
 230    }
 231
 232   /** Number of rows in the matrix.
 233       @return the number of rows in the matrix.
 234    */
 235    unsigned int Rows(void) const { return eRows; }
 236
 237   /** Number of cloumns in the matrix.
 238       @return the number of columns in the matrix.
 239    */
 240    unsigned int Cols(void) const { return eColumns; }
 241
 242   /** Transposed matrix.
 243       This function only returns the transpose of this matrix. This matrix itself
 244       remains unchanged.
 245       @return the transposed matrix.
 246    */
 247   FGMatrix33 Transposed(void) const {
 248     return FGMatrix33( data[0], data[1], data[2],
 249                        data[3], data[4], data[5],
 250                        data[6], data[7], data[8] );
 251   }
 252
 253   /** Transposes this matrix.
 254       This function only transposes this matrix. Nothing is returned.
 255    */
 256   void T(void);
 257
 258 /** Initialize the matrix.
 259     This function initializes a matrix to all 0.0.
 260  */
 261   void InitMatrix(void);
 262
 263 /** Initialize the matrix.
 264     This function initializes a matrix to user specified values.
 265  */
 266   void InitMatrix(double m11, double m12, double m13,
 267                   double m21, double m22, double m23,
 268                   double m31, double m32, double m33) {
 269     data[0] = m11;
 270     data[1] = m21;
 271     data[2] = m31;
 272     data[3] = m12;
 273     data[4] = m22;
 274     data[5] = m32;
 275     data[6] = m13;
 276     data[7] = m23;
 277     data[8] = m33;
 278   }
 279
 280   /** Returns the quaternion associated with this direction cosine (rotation) matrix.
 281   */
 282   FGQuaternion GetQuaternion(void);
 283
 284   /** Determinant of the matrix.
 285       @return the determinant of the matrix.
 286    */
 287   double Determinant(void) const;
 288
 289   /** Return if the matrix is invertible.
 290       Checks and returns if the matrix is nonsingular and thus
 291       invertible. This is done by simply computing the determinant and
 292       check if it is zero. Note that this test does not cover any
 293       instabilities caused by nearly singular matirces using finite
 294       arithmetics. It only checks exact singularity.
 295    */
 296   bool Invertible(void) const { return 0.0 != Determinant(); }
 297
 298   /** Return the inverse of the matrix.
 299       Computes and returns if the inverse of the matrix. It is computed
 300       by Cramers Rule. Also there are no checks performed if the matrix
 301       is invertible. If you are not sure that it really is check this
 302       with the @ref Invertible() call before.
 303    */
 304   FGMatrix33 Inverse(void) const;
 305
 306   /** Assignment operator.
 307
 308       @param A source matrix.
 309
 310       Copy the content of the matrix given in the argument into *this.
 311    */
 312   FGMatrix33& operator=(const FGMatrix33& A) {
 313     data[0] = A.data[0];
 314     data[1] = A.data[1];
 315     data[2] = A.data[2];
 316     data[3] = A.data[3];
 317     data[4] = A.data[4];
 318     data[5] = A.data[5];
 319     data[6] = A.data[6];
 320     data[7] = A.data[7];
 321     data[8] = A.data[8];
 322     return *this;
 323   }
 324
 325   /** Matrix vector multiplication.
 326
 327       @param v vector to multiply with.
 328       @return matric vector product.
 329
 330       Compute and return the product of the current matrix with the
 331       vector given in the argument.
 332    */
 333   FGColumnVector3 operator*(const FGColumnVector3& v) const;
 334
 335   /** Matrix subtraction.
 336
 337       @param B matrix to add to.
 338       @return difference of the matrices.
 339
 340       Compute and return the sum of the current matrix and the matrix
 341       B given in the argument.
 342   */
 343   FGMatrix33 operator-(const FGMatrix33& B) const;
 344
 345   /** Matrix addition.
 346
 347       @param B matrix to add to.
 348       @return sum of the matrices.
 349
 350       Compute and return the sum of the current matrix and the matrix
 351       B given in the argument.
 352   */
 353   FGMatrix33 operator+(const FGMatrix33& B) const;
 354
 355   /** Matrix product.
 356
 357       @param B matrix to add to.
 358       @return product of the matrices.
 359
 360       Compute and return the product of the current matrix and the matrix
 361       B given in the argument.
 362   */
 363   FGMatrix33 operator*(const FGMatrix33& B) const;
 364
 365   /** Multiply the matrix with a scalar.
 366
 367       @param scalar scalar factor to multiply with.
 368       @return scaled matrix.
 369
 370       Compute and return the product of the current matrix with the
 371       scalar value scalar given in the argument.
 372   */
 373   FGMatrix33 operator*(const double scalar) const;
 374
 375   /** Multiply the matrix with 1.0/scalar.
 376
 377       @param scalar scalar factor to divide through.
 378       @return scaled matrix.
 379
 380       Compute and return the product of the current matrix with the
 381       scalar value 1.0/scalar, where scalar is given in the argument.
 382   */
 383   FGMatrix33 operator/(const double scalar) const;
 384
 385   /** In place matrix subtraction.
 386
 387       @param B matrix to subtract.
 388       @return reference to the current matrix.
 389
 390       Compute the diffence from the current matrix and the matrix B
 391       given in the argument.
 392   */
 393   FGMatrix33& operator-=(const FGMatrix33 &B);
 394
 395   /** In place matrix addition.
 396
 397       @param B matrix to add.
 398       @return reference to the current matrix.
 399
 400       Compute the sum of the current matrix and the matrix B
 401       given in the argument.
 402   */
 403   FGMatrix33& operator+=(const FGMatrix33 &B);
 404
 405   /** In place matrix multiplication.
 406
 407       @param B matrix to multiply with.
 408       @return reference to the current matrix.
 409
 410       Compute the product of the current matrix and the matrix B
 411       given in the argument.
 412   */
 413   FGMatrix33& operator*=(const FGMatrix33 &B);
 414
 415   /** In place matrix scale.
 416
 417       @param scalar scalar value to multiply with.
 418       @return reference to the current matrix.
 419
 420       Compute the product of the current matrix and the scalar value scalar
 421       given in the argument.
 422   */
 423   FGMatrix33& operator*=(const double scalar);
 424
 425   /** In place matrix scale.
 426
 427       @param scalar scalar value to divide through.
 428       @return reference to the current matrix.
 429
 430       Compute the product of the current matrix and the scalar value
 431       1.0/scalar, where scalar is given in the argument.
 432   */
 433   FGMatrix33& operator/=(const double scalar);
 434
 435 private:
 436   double data[eRows*eColumns];
 437
 438   void Debug(int from);
 439 };
 440
 441 /** Scalar multiplication.
 442
 443     @param scalar scalar value to multiply with.
 444     @param A Matrix to multiply.
 445
 446     Multiply the Matrix with a scalar value.
 447 */
 448 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
 449   // use already defined operation.
 450   return A*scalar;
 451 }
 452
 453 /** Write matrix to a stream.
 454
 455     @param os Stream to write to.
 456     @param M Matrix to write.
 457
 458     Write the matrix to a stream.
 459 */
 460 std::ostream& operator<<(std::ostream& os, const FGMatrix33& M);
 461
 462 /** Read matrix from a stream.
 463
 464     @param os Stream to read from.
 465     @param M Matrix to initialize with the values from the stream.
 466
 467     Read matrix from a stream.
 468 */
 469 std::istream& operator>>(std::istream& is, FGMatrix33& M);
 470
 471 } // namespace JSBSim
 472
 473 #include "FGQuaternion.h"
 474
 475 #endif