src/FDM/JSBSim/FGMatrix33.cpp

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