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   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  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   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  46 }
  47
  48 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49
  50 FGColumnVector4::~FGColumnVector4(void)
  51 {
  52   if (debug_lvl & 2) cout << "Destroyed:    FGColumnVector4" << endl;
  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   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  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
  80   if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
  81
  82   return *this;
  83 }
  84
  85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  86
  87 FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
  88 {
  89   FGColumnVector4 Sum;
  90
  91   Sum(1) = C(1) + data[1];
  92   Sum(2) = C(2) + data[2];
  93   Sum(3) = C(3) + data[3];
  94   Sum(4) = C(4) + data[4];
  95   return Sum;
  96 }
  97
  98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  99
 100 void FGColumnVector4::operator+=(const FGColumnVector4& C)
 101 {
 102    data[1] += C(1);
 103    data[2] += C(2);
 104    data[3] += C(3);
 105    data[4] += C(4);
 106 }
 107
 108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109
 110 FGColumnVector4 FGColumnVector4::operator*(const double scalar)
 111 {
 112   FGColumnVector4 Product;
 113
 114   Product(1) = scalar * data[1];
 115   Product(2) = scalar * data[2];
 116   Product(3) = scalar * data[3];
 117   Product(4) = scalar * data[4];
 118
 119   return Product;
 120 }
 121
 122 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 123
 124 void FGColumnVector4::operator*=(const double scalar)
 125 {
 126   data[1] *= scalar;
 127   data[2] *= scalar;
 128   data[3] *= scalar;
 129   data[4] *= scalar;
 130 }
 131
 132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 133
 134 FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
 135 {
 136
 137   FGColumnVector4 Diff;
 138
 139   Diff(1) = data[1] - V(1);
 140   Diff(2) = data[2] - V(2);
 141   Diff(3) = data[3] - V(3);
 142   Diff(4) = data[4] - V(4);
 143
 144   return Diff;
 145 }
 146
 147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 148
 149 void FGColumnVector4::operator-=(const FGColumnVector4& V)
 150 {
 151   data[1] -= V(1);
 152   data[2] -= V(2);
 153   data[3] -= V(3);
 154   data[4] -= V(4);
 155 }
 156
 157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 158
 159 FGColumnVector4 FGColumnVector4::operator/(const double scalar)
 160 {
 161   FGColumnVector4 Quotient;
 162
 163   if (scalar != 0) {
 164           double tmp = 1.0/scalar;
 165     Quotient(1) = data[1] * tmp;
 166     Quotient(2) = data[2] * tmp;
 167     Quotient(3) = data[3] * tmp;
 168     Quotient(4) = data[4] * tmp;
 169   } else {
 170     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
 171   }
 172   return Quotient;
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176
 177 void FGColumnVector4::operator/=(const double scalar)
 178 {
 179   FGColumnVector4 Quotient;
 180
 181   if (scalar != 0) {
 182           double tmp = 1.0/scalar;
 183     data[1] *= tmp;
 184     data[2] *= tmp;
 185     data[3] *= tmp;
 186     data[4] *= tmp;
 187   } else {
 188     cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
 189   }
 190 }
 191
 192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 193
 194 FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
 195 {
 196   FGColumnVector4 Product;
 197
 198   Product(1) = scalar * C(1);
 199   Product(2) = scalar * C(2);
 200   Product(3) = scalar * C(3);
 201   Product(4) = scalar * C(4);
 202   return Product;
 203 }
 204
 205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 206
 207 double FGColumnVector4::Magnitude(void)
 208 {
 209   double num;
 210
 211   if ((data[1] == 0.00) &&
 212       (data[2] == 0.00) &&
 213       (data[3] == 0.00) &&
 214       (data[4] == 0.00))
 215   {
 216     return 0.00;
 217   } else {
 218     num  = data[1]*data[1];
 219     num += data[2]*data[2];
 220     num += data[3]*data[3];
 221     num += data[4]*data[4];
 222     return sqrt(num);
 223   }
 224 }
 225
 226 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 227
 228 FGColumnVector4 FGColumnVector4::Normalize(void)
 229 {
 230   double Mag = Magnitude();
 231
 232   if (Mag != 0) {
 233           Mag = 1.0/Mag;
 234      data[1] *= Mag;
 235      data[2] *= Mag;
 236      data[3] *= Mag;
 237      data[4] *= Mag;
 238   }
 239
 240   return *this;
 241 }
 242
 243 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 244
 245 FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
 246 {
 247   FGColumnVector4 Product;
 248
 249   Product(1) = data[1] * V(1);
 250   Product(2) = data[2] * V(2);
 251   Product(3) = data[3] * V(3);
 252   Product(4) = data[4] * V(4);
 253
 254   return Product;
 255 }
 256
 257 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 258
 259 ostream& operator<<(ostream& os, FGColumnVector4& col)
 260 {
 261   os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
 262   return os;
 263 }
 264
 265 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 266
 267 FGColumnVector4& FGColumnVector4::operator<<(const double ff)
 268 {
 269   data[rowCtr] = ff;
 270   if (++rowCtr > 4) rowCtr = 1;
 271   return *this;
 272 }
 273
 274 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 275
 276 void FGColumnVector4::Debug(int from)
 277 {
 278     //TODO: Add your source code here
 279 }
 280