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