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