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