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 using std::cout;
  49 using std::endl;
  50
  51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52 CLASS IMPLEMENTATION
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  54
  55 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56
  57 FGMatrix33::FGMatrix33(void)
  58 {
  59   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
  60     data[6] = data[7] = data[8] = 0.0;
  61
  62   Debug(0);
  63 }
  64
  65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  66
  67 std::ostream& operator<<(std::ostream& os, const FGMatrix33& M)
  68 {
  69   for (unsigned int i=1; i<=M.Rows(); i++) {
  70     for (unsigned int j=1; j<=M.Cols(); j++) {
  71       if (i == M.Rows() && j == M.Cols())
  72         os << M(i,j);
  73       else
  74         os << M(i,j) << ", ";
  75     }
  76   }
  77   return os;
  78 }
  79
  80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  81
  82 std::istream& operator>>(std::istream& is, FGMatrix33& M)
  83 {
  84   for (unsigned int i=1; i<=M.Rows(); i++) {
  85     for (unsigned int j=1; j<=M.Cols(); j++) {
  86       is >> M(i,j);
  87     }
  88   }
  89   return is;
  90 }
  91
  92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93
  94 double FGMatrix33::Determinant(void) const {
  95   return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
  96     + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
  97     - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
  98 }
  99
 100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 101
 102 FGMatrix33 FGMatrix33::Inverse(void) const {
 103   // Compute the inverse of a general matrix using Cramers rule.
 104   // I guess googling for cramers rule gives tons of references
 105   // for this. :)
 106   double rdet = 1.0/Determinant();
 107
 108   double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
 109   double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
 110   double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
 111   double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
 112   double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
 113   double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
 114   double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
 115   double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
 116   double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
 117
 118   return FGMatrix33( i11, i12, i13,
 119                      i21, i22, i23,
 120                      i31, i32, i33 );
 121 }
 122
 123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 124
 125 void FGMatrix33::InitMatrix(void)
 126 {
 127   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 128     data[6] = data[7] = data[8] = 0.0;
 129 }
 130
 131 // *****************************************************************************
 132 // binary operators ************************************************************
 133 // *****************************************************************************
 134
 135 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 136 {
 137   return FGMatrix33( Entry(1,1) - M(1,1),
 138                      Entry(1,2) - M(1,2),
 139                      Entry(1,3) - M(1,3),
 140                      Entry(2,1) - M(2,1),
 141                      Entry(2,2) - M(2,2),
 142                      Entry(2,3) - M(2,3),
 143                      Entry(3,1) - M(3,1),
 144                      Entry(3,2) - M(3,2),
 145                      Entry(3,3) - M(3,3) );
 146 }
 147
 148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 149
 150 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 151 {
 152   data[0] -= M.data[0];
 153   data[1] -= M.data[1];
 154   data[2] -= M.data[2];
 155   data[3] -= M.data[3];
 156   data[4] -= M.data[4];
 157   data[5] -= M.data[5];
 158   data[6] -= M.data[6];
 159   data[7] -= M.data[7];
 160   data[8] -= M.data[8];
 161
 162   return *this;
 163 }
 164
 165 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 166
 167 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 168 {
 169   return FGMatrix33( Entry(1,1) + M(1,1),
 170                      Entry(1,2) + M(1,2),
 171                      Entry(1,3) + M(1,3),
 172                      Entry(2,1) + M(2,1),
 173                      Entry(2,2) + M(2,2),
 174                      Entry(2,3) + M(2,3),
 175                      Entry(3,1) + M(3,1),
 176                      Entry(3,2) + M(3,2),
 177                      Entry(3,3) + M(3,3) );
 178 }
 179
 180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 181
 182 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 183 {
 184   Entry(1,1) += M(1,1);
 185   Entry(1,2) += M(1,2);
 186   Entry(1,3) += M(1,3);
 187   Entry(2,1) += M(2,1);
 188   Entry(2,2) += M(2,2);
 189   Entry(2,3) += M(2,3);
 190   Entry(3,1) += M(3,1);
 191   Entry(3,2) += M(3,2);
 192   Entry(3,3) += M(3,3);
 193
 194   return *this;
 195 }
 196
 197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 198
 199 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 200 {
 201   return FGMatrix33( scalar * Entry(1,1),
 202                      scalar * Entry(1,2),
 203                      scalar * Entry(1,3),
 204                      scalar * Entry(2,1),
 205                      scalar * Entry(2,2),
 206                      scalar * Entry(2,3),
 207                      scalar * Entry(3,1),
 208                      scalar * Entry(3,2),
 209                      scalar * Entry(3,3) );
 210 }
 211
 212 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 213
 214 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 215 {
 216   return FGMatrix33( scalar * M(1,1),
 217                      scalar * M(1,2),
 218                      scalar * M(1,3),
 219                      scalar * M(2,1),
 220                      scalar * M(2,2),
 221                      scalar * M(2,3),
 222                      scalar * M(3,1),
 223                      scalar * M(3,2),
 224                      scalar * M(3,3) );
 225 }
 226
 227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 228
 229 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 230 {
 231   Entry(1,1) *= scalar;
 232   Entry(1,2) *= scalar;
 233   Entry(1,3) *= scalar;
 234   Entry(2,1) *= scalar;
 235   Entry(2,2) *= scalar;
 236   Entry(2,3) *= scalar;
 237   Entry(3,1) *= scalar;
 238   Entry(3,2) *= scalar;
 239   Entry(3,3) *= scalar;
 240
 241   return *this;
 242 }
 243
 244 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 245
 246 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 247 {
 248   // FIXME: Make compiler friendlier
 249   FGMatrix33 Product;
 250
 251   Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
 252   Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
 253   Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
 254   Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
 255   Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
 256   Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
 257   Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
 258   Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
 259   Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
 260
 261   return Product;
 262 }
 263
 264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 265
 266 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 267 {
 268   // FIXME: Make compiler friendlier
 269   double a,b,c;
 270
 271   a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
 272   Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 273   Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 274   Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 275
 276   a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
 277   Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 278   Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 279   Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 280
 281   a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
 282   Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 283   Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 284   Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 285
 286   return *this;
 287 }
 288
 289 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 290
 291 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 292 {
 293   FGMatrix33 Quot;
 294
 295   if ( scalar != 0 ) {
 296     double tmp = 1.0/scalar;
 297     Quot(1,1) = Entry(1,1) * tmp;
 298     Quot(1,2) = Entry(1,2) * tmp;
 299     Quot(1,3) = Entry(1,3) * tmp;
 300     Quot(2,1) = Entry(2,1) * tmp;
 301     Quot(2,2) = Entry(2,2) * tmp;
 302     Quot(2,3) = Entry(2,3) * tmp;
 303     Quot(3,1) = Entry(3,1) * tmp;
 304     Quot(3,2) = Entry(3,2) * tmp;
 305     Quot(3,3) = Entry(3,3) * tmp;
 306   } else {
 307     MatrixException mE;
 308     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 309     throw mE;
 310   }
 311   return Quot;
 312 }
 313
 314 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 315
 316 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 317 {
 318   if ( scalar != 0 ) {
 319     double tmp = 1.0/scalar;
 320     Entry(1,1) *= tmp;
 321     Entry(1,2) *= tmp;
 322     Entry(1,3) *= tmp;
 323     Entry(2,1) *= tmp;
 324     Entry(2,2) *= tmp;
 325     Entry(2,3) *= tmp;
 326     Entry(3,1) *= tmp;
 327     Entry(3,2) *= tmp;
 328     Entry(3,3) *= tmp;
 329   } else {
 330     MatrixException mE;
 331     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 332     throw mE;
 333   }
 334   return *this;
 335 }
 336
 337 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 338
 339 void FGMatrix33::T(void)
 340 {
 341   for (unsigned int i=1; i<=3; i++) {
 342     for (unsigned int j=i+1; j<=3; j++) {
 343       double tmp = Entry(i,j);
 344       Entry(i,j) = Entry(j,i);
 345       Entry(j,i) = tmp;
 346     }
 347   }
 348 }
 349
 350 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 351
 352 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
 353   double tmp1 = v(1)*Entry(1,1);
 354   double tmp2 = v(1)*Entry(2,1);
 355   double tmp3 = v(1)*Entry(3,1);
 356
 357   tmp1 += v(2)*Entry(1,2);
 358   tmp2 += v(2)*Entry(2,2);
 359   tmp3 += v(2)*Entry(3,2);
 360
 361   tmp1 += v(3)*Entry(1,3);
 362   tmp2 += v(3)*Entry(2,3);
 363   tmp3 += v(3)*Entry(3,3);
 364
 365   return FGColumnVector3( tmp1, tmp2, tmp3 );
 366 }
 367
 368 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 369 //    The bitmasked value choices are as follows:
 370 //    unset: In this case (the default) JSBSim would only print
 371 //       out the normally expected messages, essentially echoing
 372 //       the config files as they are read. If the environment
 373 //       variable is not set, debug_lvl is set to 1 internally
 374 //    0: This requests JSBSim not to output any messages
 375 //       whatsoever.
 376 //    1: This value explicity requests the normal JSBSim
 377 //       startup messages
 378 //    2: This value asks for a message to be printed out when
 379 //       a class is instantiated
 380 //    4: When this value is set, a message is displayed when a
 381 //       FGModel object executes its Run() method
 382 //    8: When this value is set, various runtime state variables
 383 //       are printed out periodically
 384 //    16: When set various parameters are sanity checked and
 385 //       a message is printed out when they go out of bounds
 386
 387 void FGMatrix33::Debug(int from)
 388 {
 389   if (debug_lvl <= 0) return;
 390
 391   if (debug_lvl & 1) { // Standard console startup message output
 392     if (from == 0) { // Constructor
 393
 394     }
 395   }
 396   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 397     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 398     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 399   }
 400   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 401   }
 402   if (debug_lvl & 8 ) { // Runtime state variables
 403   }
 404   if (debug_lvl & 16) { // Sanity checking
 405   }
 406   if (debug_lvl & 64) {
 407     if (from == 0) { // Constructor
 408       cout << IdSrc << endl;
 409       cout << IdHdr << endl;
 410     }
 411   }
 412 }
 413 }