src/FDM/JSBSim/math/FGMatrix33.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Module: FGMatrix33.cpp
   4 Author: Tony Peden, Jon Berndt, Mathias Frolich
   5 Date started: 1998
   6 Purpose: FGMatrix33 class
   7 Called by: Various
   8
   9  ------------- Copyright (C) 1998 by the authors above -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU Lesser General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30
  31 HISTORY
  32 --------------------------------------------------------------------------------
  33 ??/??/??   TP   Created
  34 03/16/2000 JSB  Added exception throwing
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGMatrix33.h"
  41 #include "FGColumnVector3.h"
  42
  43 namespace JSBSim {
  44
  45 static const char *IdSrc = "$Id$";
  46 static const char *IdHdr = ID_MATRIX33;
  47
  48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49 CLASS IMPLEMENTATION
  50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  51
  52 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53
  54 FGMatrix33::FGMatrix33(void)
  55 {
  56   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
  57     data[6] = data[7] = data[8] = 0.0;
  58
  59   Debug(0);
  60 }
  61
  62 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  63
  64 ostream& operator<<(ostream& os, const FGMatrix33& M)
  65 {
  66   for (unsigned int i=1; i<=M.Rows(); i++) {
  67     for (unsigned int j=1; j<=M.Cols(); j++) {
  68       if (i == M.Rows() && j == M.Cols())
  69         os << M(i,j);
  70       else
  71         os << M(i,j) << ", ";
  72     }
  73   }
  74   return os;
  75 }
  76
  77 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78
  79 istream& operator>>(istream& is, FGMatrix33& M)
  80 {
  81   for (unsigned int i=1; i<=M.Rows(); i++) {
  82     for (unsigned int j=1; j<=M.Cols(); j++) {
  83       is >> M(i,j);
  84     }
  85   }
  86   return is;
  87 }
  88
  89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  90
  91 double FGMatrix33::Determinant(void) const {
  92   return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
  93     + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
  94     - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
  95 }
  96
  97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  98
  99 FGMatrix33 FGMatrix33::Inverse(void) const {
 100   // Compute the inverse of a general matrix using Cramers rule.
 101   // I guess googling for cramers rule gives tons of references
 102   // for this. :)
 103   double rdet = 1.0/Determinant();
 104
 105   double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
 106   double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
 107   double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
 108   double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
 109   double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
 110   double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
 111   double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
 112   double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
 113   double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
 114
 115   return FGMatrix33( i11, i12, i13,
 116                      i21, i22, i23,
 117                      i31, i32, i33 );
 118 }
 119
 120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 121
 122 void FGMatrix33::InitMatrix(void)
 123 {
 124   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 125     data[6] = data[7] = data[8] = 0.0;
 126 }
 127
 128 // *****************************************************************************
 129 // binary operators ************************************************************
 130 // *****************************************************************************
 131
 132 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 133 {
 134   return FGMatrix33( Entry(1,1) - M(1,1),
 135                      Entry(1,2) - M(1,2),
 136                      Entry(1,3) - M(1,3),
 137                      Entry(2,1) - M(2,1),
 138                      Entry(2,2) - M(2,2),
 139                      Entry(2,3) - M(2,3),
 140                      Entry(3,1) - M(3,1),
 141                      Entry(3,2) - M(3,2),
 142                      Entry(3,3) - M(3,3) );
 143 }
 144
 145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 146
 147 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 148 {
 149   data[0] -= M.data[0];
 150   data[1] -= M.data[1];
 151   data[2] -= M.data[2];
 152   data[3] -= M.data[3];
 153   data[4] -= M.data[4];
 154   data[5] -= M.data[5];
 155   data[6] -= M.data[6];
 156   data[7] -= M.data[7];
 157   data[8] -= M.data[8];
 158
 159   return *this;
 160 }
 161
 162 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 163
 164 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 165 {
 166   return FGMatrix33( Entry(1,1) + M(1,1),
 167                      Entry(1,2) + M(1,2),
 168                      Entry(1,3) + M(1,3),
 169                      Entry(2,1) + M(2,1),
 170                      Entry(2,2) + M(2,2),
 171                      Entry(2,3) + M(2,3),
 172                      Entry(3,1) + M(3,1),
 173                      Entry(3,2) + M(3,2),
 174                      Entry(3,3) + M(3,3) );
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178
 179 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 180 {
 181   Entry(1,1) += M(1,1);
 182   Entry(1,2) += M(1,2);
 183   Entry(1,3) += M(1,3);
 184   Entry(2,1) += M(2,1);
 185   Entry(2,2) += M(2,2);
 186   Entry(2,3) += M(2,3);
 187   Entry(3,1) += M(3,1);
 188   Entry(3,2) += M(3,2);
 189   Entry(3,3) += M(3,3);
 190
 191   return *this;
 192 }
 193
 194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 195
 196 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 197 {
 198   return FGMatrix33( scalar * Entry(1,1),
 199                      scalar * Entry(1,2),
 200                      scalar * Entry(1,3),
 201                      scalar * Entry(2,1),
 202                      scalar * Entry(2,2),
 203                      scalar * Entry(2,3),
 204                      scalar * Entry(3,1),
 205                      scalar * Entry(3,2),
 206                      scalar * Entry(3,3) );
 207 }
 208
 209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 210
 211 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 212 {
 213   return FGMatrix33( scalar * M(1,1),
 214                      scalar * M(1,2),
 215                      scalar * M(1,3),
 216                      scalar * M(2,1),
 217                      scalar * M(2,2),
 218                      scalar * M(2,3),
 219                      scalar * M(3,1),
 220                      scalar * M(3,2),
 221                      scalar * M(3,3) );
 222 }
 223
 224 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 225
 226 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 227 {
 228   Entry(1,1) *= scalar;
 229   Entry(1,2) *= scalar;
 230   Entry(1,3) *= scalar;
 231   Entry(2,1) *= scalar;
 232   Entry(2,2) *= scalar;
 233   Entry(2,3) *= scalar;
 234   Entry(3,1) *= scalar;
 235   Entry(3,2) *= scalar;
 236   Entry(3,3) *= scalar;
 237
 238   return *this;
 239 }
 240
 241 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 242
 243 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 244 {
 245   // FIXME: Make compiler friendlier
 246   FGMatrix33 Product;
 247
 248   Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
 249   Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
 250   Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
 251   Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
 252   Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
 253   Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
 254   Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
 255   Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
 256   Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
 257
 258   return Product;
 259 }
 260
 261 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 262
 263 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 264 {
 265   // FIXME: Make compiler friendlier
 266   double a,b,c;
 267
 268   a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
 269   Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 270   Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 271   Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 272
 273   a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
 274   Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 275   Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 276   Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 277
 278   a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
 279   Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 280   Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 281   Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 282
 283   return *this;
 284 }
 285
 286 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 287
 288 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 289 {
 290   FGMatrix33 Quot;
 291
 292   if ( scalar != 0 ) {
 293     double tmp = 1.0/scalar;
 294     Quot(1,1) = Entry(1,1) * tmp;
 295     Quot(1,2) = Entry(1,2) * tmp;
 296     Quot(1,3) = Entry(1,3) * tmp;
 297     Quot(2,1) = Entry(2,1) * tmp;
 298     Quot(2,2) = Entry(2,2) * tmp;
 299     Quot(2,3) = Entry(2,3) * tmp;
 300     Quot(3,1) = Entry(3,1) * tmp;
 301     Quot(3,2) = Entry(3,2) * tmp;
 302     Quot(3,3) = Entry(3,3) * tmp;
 303   } else {
 304     MatrixException mE;
 305     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 306     throw mE;
 307   }
 308   return Quot;
 309 }
 310
 311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 312
 313 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 314 {
 315   if ( scalar != 0 ) {
 316     double tmp = 1.0/scalar;
 317     Entry(1,1) *= tmp;
 318     Entry(1,2) *= tmp;
 319     Entry(1,3) *= tmp;
 320     Entry(2,1) *= tmp;
 321     Entry(2,2) *= tmp;
 322     Entry(2,3) *= tmp;
 323     Entry(3,1) *= tmp;
 324     Entry(3,2) *= tmp;
 325     Entry(3,3) *= tmp;
 326   } else {
 327     MatrixException mE;
 328     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 329     throw mE;
 330   }
 331   return *this;
 332 }
 333
 334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 335
 336 void FGMatrix33::T(void)
 337 {
 338   for (unsigned int i=1; i<=3; i++) {
 339     for (unsigned int j=i+1; j<=3; j++) {
 340       double tmp = Entry(i,j);
 341       Entry(i,j) = Entry(j,i);
 342       Entry(j,i) = tmp;
 343     }
 344   }
 345 }
 346
 347 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 348
 349 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
 350   double tmp1 = v(1)*Entry(1,1);
 351   double tmp2 = v(1)*Entry(2,1);
 352   double tmp3 = v(1)*Entry(3,1);
 353
 354   tmp1 += v(2)*Entry(1,2);
 355   tmp2 += v(2)*Entry(2,2);
 356   tmp3 += v(2)*Entry(3,2);
 357
 358   tmp1 += v(3)*Entry(1,3);
 359   tmp2 += v(3)*Entry(2,3);
 360   tmp3 += v(3)*Entry(3,3);
 361
 362   return FGColumnVector3( tmp1, tmp2, tmp3 );
 363 }
 364
 365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 366 //    The bitmasked value choices are as follows:
 367 //    unset: In this case (the default) JSBSim would only print
 368 //       out the normally expected messages, essentially echoing
 369 //       the config files as they are read. If the environment
 370 //       variable is not set, debug_lvl is set to 1 internally
 371 //    0: This requests JSBSim not to output any messages
 372 //       whatsoever.
 373 //    1: This value explicity requests the normal JSBSim
 374 //       startup messages
 375 //    2: This value asks for a message to be printed out when
 376 //       a class is instantiated
 377 //    4: When this value is set, a message is displayed when a
 378 //       FGModel object executes its Run() method
 379 //    8: When this value is set, various runtime state variables
 380 //       are printed out periodically
 381 //    16: When set various parameters are sanity checked and
 382 //       a message is printed out when they go out of bounds
 383
 384 void FGMatrix33::Debug(int from)
 385 {
 386   if (debug_lvl <= 0) return;
 387
 388   if (debug_lvl & 1) { // Standard console startup message output
 389     if (from == 0) { // Constructor
 390
 391     }
 392   }
 393   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 394     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 395     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 396   }
 397   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 398   }
 399   if (debug_lvl & 8 ) { // Runtime state variables
 400   }
 401   if (debug_lvl & 16) { // Sanity checking
 402   }
 403   if (debug_lvl & 64) {
 404     if (from == 0) { // Constructor
 405       cout << IdSrc << endl;
 406       cout << IdHdr << endl;
 407     }
 408   }
 409 }
 410 }