src/FDM/JSBSim/FGColumnVector4.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Module: FGMatrix33.cpp
   4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
   5 Date started: 1998
   6 Purpose: FGMatrix33 class
   7 Called by: Various
   8
   9 FUNCTIONAL DESCRIPTION
  10 --------------------------------------------------------------------------------
  11
  12 HISTORY
  13 --------------------------------------------------------------------------------
  14 ??/??/??   TP   Created
  15 03/16/2000 JSB  Added exception throwing
  16
  17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  18 INCLUDES
  19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  20
  21 #include "FGColumnVector4.h"
  22
  23 static const char *IdSrc = "$Id$";
  24 static const char *IdHdr = ID_COLUMNVECTOR4;
  25
  26 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  27 CLASS IMPLEMENTATION
  28 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  29
  30 FGColumnVector4::FGColumnVector4(void)
  31 {
  32   rowCtr = 1;
  33   data[1]=0;data[2]=0;data[3]=0;data[4]=0;
  34
  35   Debug(0);
  36 }
  37
  38 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  39
  40 FGColumnVector4::FGColumnVector4(double A, double B, double C, double D)
  41 {
  42   rowCtr = 1;
  43   data[1]=0;data[2]=0;data[3]=0;data[4]=0;
  44
  45   Debug(0);
  46 }
  47
  48 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49
  50 FGColumnVector4::~FGColumnVector4(void)
  51 {
  52   Debug(1);
  53 }
  54
  55
  56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  57
  58 FGColumnVector4::FGColumnVector4(const FGColumnVector4& b)
  59 {
  60   data[1] = b.data[1];
  61   data[2] = b.data[2];
  62   data[3] = b.data[3];
  63   data[4] = b.data[4];
  64
  65   rowCtr = 1;
  66
  67   Debug(0);
  68 }
  69
  70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  71
  72 FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b)
  73 {
  74   data[1] = b.data[1];
  75   data[2] = b.data[2];
  76   data[3] = b.data[3];
  77   data[4] = b.data[4];
  78   rowCtr = 1;
  79   return *this;
  80 }
  81
  82 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  83
  84 FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
  85 {
  86   FGColumnVector4 Sum;
  87
  88   Sum(1) = C(1) + data[1];
  89   Sum(2) = C(2) + data[2];
  90   Sum(3) = C(3) + data[3];
  91   Sum(4) = C(4) + data[4];
  92   return Sum;
  93 }
  94
  95 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  96
  97 void FGColumnVector4::operator+=(const FGColumnVector4& C)
  98 {
  99    data[1] += C(1);
 100    data[2] += C(2);
 101    data[3] += C(3);
 102    data[4] += C(4);
 103 }
 104
 105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 106
 107 FGColumnVector4 FGColumnVector4::operator*(const double scalar)
 108 {
 109   FGColumnVector4 Product;
 110
 111   Product(1) = scalar * data[1];
 112   Product(2) = scalar * data[2];
 113   Product(3) = scalar * data[3];
 114   Product(4) = scalar * data[4];
 115
 116   return Product;
 117 }
 118
 119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 120
 121 void FGColumnVector4::operator*=(const double scalar)
 122 {
 123   data[1] *= scalar;
 124   data[2] *= scalar;
 125   data[3] *= scalar;
 126   data[4] *= scalar;
 127 }
 128
 129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 130
 131 FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
 132 {
 133
 134   FGColumnVector4 Diff;
 135
 136   Diff(1) = data[1] - V(1);
 137   Diff(2) = data[2] - V(2);
 138   Diff(3) = data[3] - V(3);
 139   Diff(4) = data[4] - V(4);
 140
 141   return Diff;
 142 }
 143
 144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 145
 146 void FGColumnVector4::operator-=(const FGColumnVector4& V)
 147 {
 148   data[1] -= V(1);
 149   data[2] -= V(2);
 150   data[3] -= V(3);
 151   data[4] -= V(4);
 152 }
 153
 154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 155
 156 FGColumnVector4 FGColumnVector4::operator/(const double scalar)
 157 {
 158   FGColumnVector4 Quotient;
 159
 160   if (scalar != 0) {
 161           double tmp = 1.0/scalar;
 162     Quotient(1) = data[1] * tmp;
 163     Quotient(2) = data[2] * tmp;
 164     Quotient(3) = data[3] * tmp;
 165     Quotient(4) = data[4] * tmp;
 166   } else {
 167     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
 168   }
 169   return Quotient;
 170 }
 171
 172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 173
 174 void FGColumnVector4::operator/=(const double scalar)
 175 {
 176   FGColumnVector4 Quotient;
 177
 178   if (scalar != 0) {
 179           double tmp = 1.0/scalar;
 180     data[1] *= tmp;
 181     data[2] *= tmp;
 182     data[3] *= tmp;
 183     data[4] *= tmp;
 184   } else {
 185     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
 186   }
 187 }
 188
 189 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 190
 191 FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
 192 {
 193   FGColumnVector4 Product;
 194
 195   Product(1) = scalar * C(1);
 196   Product(2) = scalar * C(2);
 197   Product(3) = scalar * C(3);
 198   Product(4) = scalar * C(4);
 199   return Product;
 200 }
 201
 202 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 203
 204 double FGColumnVector4::Magnitude(void)
 205 {
 206   double num;
 207
 208   if ((data[1] == 0.00) &&
 209       (data[2] == 0.00) &&
 210       (data[3] == 0.00) &&
 211       (data[4] == 0.00))
 212   {
 213     return 0.00;
 214   } else {
 215     num  = data[1]*data[1];
 216     num += data[2]*data[2];
 217     num += data[3]*data[3];
 218     num += data[4]*data[4];
 219     return sqrt(num);
 220   }
 221 }
 222
 223 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 224
 225 FGColumnVector4 FGColumnVector4::Normalize(void)
 226 {
 227   double Mag = Magnitude();
 228
 229   if (Mag != 0) {
 230           Mag = 1.0/Mag;
 231      data[1] *= Mag;
 232      data[2] *= Mag;
 233      data[3] *= Mag;
 234      data[4] *= Mag;
 235   }
 236
 237   return *this;
 238 }
 239
 240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 241
 242 FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
 243 {
 244   FGColumnVector4 Product;
 245
 246   Product(1) = data[1] * V(1);
 247   Product(2) = data[2] * V(2);
 248   Product(3) = data[3] * V(3);
 249   Product(4) = data[4] * V(4);
 250
 251   return Product;
 252 }
 253
 254 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 255
 256 ostream& operator<<(ostream& os, FGColumnVector4& col)
 257 {
 258   os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
 259   return os;
 260 }
 261
 262 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 263
 264 FGColumnVector4& FGColumnVector4::operator<<(const double ff)
 265 {
 266   data[rowCtr] = ff;
 267   if (++rowCtr > 4) rowCtr = 1;
 268   return *this;
 269 }
 270 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 271 //    The bitmasked value choices are as follows:
 272 //    unset: In this case (the default) JSBSim would only print
 273 //       out the normally expected messages, essentially echoing
 274 //       the config files as they are read. If the environment
 275 //       variable is not set, debug_lvl is set to 1 internally
 276 //    0: This requests JSBSim not to output any messages
 277 //       whatsoever.
 278 //    1: This value explicity requests the normal JSBSim
 279 //       startup messages
 280 //    2: This value asks for a message to be printed out when
 281 //       a class is instantiated
 282 //    4: When this value is set, a message is displayed when a
 283 //       FGModel object executes its Run() method
 284 //    8: When this value is set, various runtime state variables
 285 //       are printed out periodically
 286 //    16: When set various parameters are sanity checked and
 287 //       a message is printed out when they go out of bounds
 288
 289 void FGColumnVector4::Debug(int from)
 290 {
 291   if (debug_lvl <= 0) return;
 292
 293   if (debug_lvl & 1) { // Standard console startup message output
 294   }
 295   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 296     if (from == 0) cout << "Instantiated: FGColumnVector4" << endl;
 297     if (from == 1) cout << "Destroyed:    FGColumnVector4" << endl;
 298   }
 299   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 300   }
 301   if (debug_lvl & 8 ) { // Runtime state variables
 302   }
 303   if (debug_lvl & 16) { // Sanity checking
 304   }
 305   if (debug_lvl & 64) { // Sanity checking
 306     if (from == 0) { // Constructor
 307       cout << IdSrc << endl;
 308       cout << IdHdr << endl;
 309     }
 310   }
 311 }
 312
 313