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   data = new double[5];
  33   rowCtr = 1;
  34   //cout << "Allocated: " <<  data << endl;
  35   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  36 }
  37
  38 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  39
  40 FGColumnVector4::FGColumnVector4(int m)
  41 {
  42   data = new double[5];
  43   rowCtr = 1;
  44   data[1]=0;data[2]=0;data[3]=0;data[4]=0;
  45   //cout << "Allocated: " <<  data << endl;
  46   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  47 }
  48
  49 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  50
  51 FGColumnVector4::~FGColumnVector4(void)
  52 {
  53   //cout << "Freed: " << data << endl;
  54   delete[] data;
  55   data = NULL;
  56   if (debug_lvl & 2) cout << "Destroyed:    FGColumnVector4" << endl;
  57 }
  58
  59
  60 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  61
  62 FGColumnVector4::FGColumnVector4(const FGColumnVector4& b)
  63 {
  64   data = new double[5];
  65   data[1] = b.data[1];
  66   data[2] = b.data[2];
  67   data[3] = b.data[3];
  68   data[4] = b.data[4];
  69
  70   rowCtr = 1;
  71
  72   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  73 }
  74
  75 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  76
  77 FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b)
  78 {
  79   data = new double[5];
  80   data[1] = b.data[1];
  81   data[2] = b.data[2];
  82   data[3] = b.data[3];
  83   data[4] = b.data[4];
  84   rowCtr = 1;
  85
  86   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  87
  88   return *this;
  89 }
  90
  91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  92
  93 FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
  94 {
  95   FGColumnVector4 Sum;
  96
  97   Sum(1) = C(1) + data[1];
  98   Sum(2) = C(2) + data[2];
  99   Sum(3) = C(3) + data[3];
 100   Sum(4) = C(4) + data[4];
 101   return Sum;
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 void FGColumnVector4::operator+=(const FGColumnVector4& C)
 107 {
 108    data[1] += C(1);
 109    data[2] += C(2);
 110    data[3] += C(3);
 111    data[4] += C(4);
 112 }
 113
 114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 115
 116 FGColumnVector4 FGColumnVector4::operator*(const double scalar)
 117 {
 118   FGColumnVector4 Product;
 119
 120   Product(1) = scalar * data[1];
 121   Product(2) = scalar * data[2];
 122   Product(3) = scalar * data[3];
 123   Product(4) = scalar * data[4];
 124
 125   return Product;
 126 }
 127
 128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 129
 130 void FGColumnVector4::operator*=(const double scalar)
 131 {
 132   data[1] *= scalar;
 133   data[2] *= scalar;
 134   data[3] *= scalar;
 135   data[4] *= scalar;
 136 }
 137
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139
 140 FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
 141 {
 142
 143   FGColumnVector4 Diff;
 144
 145   Diff(1) = data[1] - V(1);
 146   Diff(2) = data[2] - V(2);
 147   Diff(3) = data[3] - V(3);
 148   Diff(4) = data[4] - V(4);
 149
 150   return Diff;
 151 }
 152
 153 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 154
 155 void FGColumnVector4::operator-=(const FGColumnVector4& V)
 156 {
 157   data[1] -= V(1);
 158   data[2] -= V(2);
 159   data[3] -= V(3);
 160   data[4] -= V(4);
 161 }
 162
 163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 164
 165 FGColumnVector4 FGColumnVector4::operator/(const double scalar)
 166 {
 167   FGColumnVector4 Quotient;
 168
 169   if (scalar != 0) {
 170           double tmp = 1.0/scalar;
 171     Quotient(1) = data[1] * tmp;
 172     Quotient(2) = data[2] * tmp;
 173     Quotient(3) = data[3] * tmp;
 174     Quotient(4) = data[4] * tmp;
 175   } else {
 176     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
 177   }
 178   return Quotient;
 179 }
 180
 181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 182
 183 void FGColumnVector4::operator/=(const double scalar)
 184 {
 185   FGColumnVector4 Quotient;
 186
 187   if (scalar != 0) {
 188           double tmp = 1.0/scalar;
 189     data[1] *= tmp;
 190     data[2] *= tmp;
 191     data[3] *= tmp;
 192     data[4] *= tmp;
 193   } else {
 194     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
 195   }
 196 }
 197
 198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 199
 200 FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
 201 {
 202   FGColumnVector4 Product;
 203
 204   Product(1) = scalar * C(1);
 205   Product(2) = scalar * C(2);
 206   Product(3) = scalar * C(3);
 207   Product(4) = scalar * C(4);
 208   return Product;
 209 }
 210
 211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 212
 213 float FGColumnVector4::Magnitude(void)
 214 {
 215   double num;
 216
 217   if ((data[1] == 0.00) &&
 218       (data[2] == 0.00) &&
 219       (data[3] == 0.00) &&
 220       (data[4] == 0.00))
 221   {
 222     return 0.00;
 223   } else {
 224     num  = data[1]*data[1];
 225     num += data[2]*data[2];
 226     num += data[3]*data[3];
 227     num += data[4]*data[4];
 228     return sqrt(num);
 229   }
 230 }
 231
 232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 233
 234 FGColumnVector4 FGColumnVector4::Normalize(void)
 235 {
 236   double Mag = Magnitude();
 237
 238   if (Mag != 0) {
 239           Mag = 1.0/Mag;
 240      data[1] *= Mag;
 241      data[2] *= Mag;
 242      data[3] *= Mag;
 243      data[4] *= Mag;
 244   }
 245
 246   return *this;
 247 }
 248
 249 /* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 250
 251 FGColumnVector4 FGColumnVector4::operator*(const FGColumnVector4& V)
 252 {
 253   FGColumnVector4 Product;
 254
 255   Product(1) = data[2] * V(3) - data[3] * V(2);
 256   Product(2) = data[3] * V(1) - data[1] * V(3);
 257   Product(3) = data[1] * V(2) - data[2] * V(1);
 258
 259   return Product;
 260 } */
 261
 262
 263 /* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 264
 265 void FGColumnVector4::operator*=(const FGColumnVector4& V)
 266 {
 267   double a,b,c;
 268   a = data[1]; b=data[2]; c=data[3];
 269
 270   data[1] = b * V(3) - c * V(2);
 271   data[2] = c * V(1) - a * V(3);
 272   data[3] = a * V(2) - b * V(1);
 273
 274 }
 275  */
 276
 277 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 278
 279 FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
 280 {
 281   FGColumnVector4 Product;
 282
 283   Product(1) = data[1] * V(1);
 284   Product(2) = data[2] * V(2);
 285   Product(3) = data[3] * V(3);
 286   Product(4) = data[4] * V(4);
 287
 288   return Product;
 289 }
 290
 291 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 292
 293 void FGColumnVector4::Debug(void)
 294 {
 295     //TODO: Add your source code here
 296 }
 297
 298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 299
 300 ostream& operator<<(ostream& os, FGColumnVector4& col)
 301 {
 302   os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
 303   return os;
 304 }
 305
 306 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 307
 308 FGColumnVector4& FGColumnVector4::operator<<(const float ff)
 309 {
 310   data[rowCtr] = ff;
 311   if (++rowCtr > 4 )
 312       rowCtr = 1;
 313   return *this;
 314 }
 315