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