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