src/FDM/JSBSim/FGColumnVector3.cpp

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