1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
6 Purpose: FGMatrix33 class
10 --------------------------------------------------------------------------------
13 --------------------------------------------------------------------------------
15 03/16/2000 JSB Added exception throwing
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
21 #include "FGColumnVector3.h"
22 #include "FGMatrix33.h"
25 static const char *IdSrc = "$Id$";
26 static const char *IdHdr = ID_COLUMNVECTOR3;
28 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
32 FGColumnVector3::FGColumnVector3(void)
35 data[0]=0; data[1]=0; data[2]=0; data[3]=0;
37 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
40 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42 FGColumnVector3::FGColumnVector3(int m)
45 data[0]=0; data[1]=0; data[2]=0; data[3]=0;
47 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
50 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 FGColumnVector3::~FGColumnVector3(void)
54 if (debug_lvl & 2) cout << "Destroyed: FGColumnVector3" << endl;
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 FGColumnVector3::FGColumnVector3(const FGColumnVector3& b)
67 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72 FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b)
79 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
85 /* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 double& FGColumnVector3::operator()(int m) const
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95 /* FGColumnVector3 operator*(const FGMatrix33& Mat, FGColumnVector3& Col)
97 FGColumnVector3 Product;
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);
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C)
112 Sum(1) = C(1) + data[1];
113 Sum(2) = C(2) + data[2];
114 Sum(3) = C(3) + data[3];
119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
121 void FGColumnVector3::operator+=(const FGColumnVector3& C)
128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130 FGColumnVector3 FGColumnVector3::operator*(const double scalar)
132 FGColumnVector3 Product;
134 Product(1) = scalar * data[1];
135 Product(2) = scalar * data[2];
136 Product(3) = scalar * data[3];
141 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
143 void FGColumnVector3::operator*=(const double scalar)
150 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
152 FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V)
155 FGColumnVector3 Diff;
157 Diff(1) = data[1] - V(1);
158 Diff(2) = data[2] - V(2);
159 Diff(3) = data[3] - V(3);
164 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
166 void FGColumnVector3::operator-=(const FGColumnVector3& V)
173 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
175 FGColumnVector3 FGColumnVector3::operator/(const double scalar)
177 FGColumnVector3 Quotient;
180 double tmp = 1.0/scalar;
181 Quotient(1) = data[1] * tmp;
182 Quotient(2) = data[2] * tmp;
183 Quotient(3) = data[3] * tmp;
185 cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl;
190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
192 void FGColumnVector3::operator/=(const double scalar)
194 FGColumnVector3 Quotient;
197 double tmp = 1.0/scalar;
202 cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl;
206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
208 FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C)
210 FGColumnVector3 Product;
212 Product(1) = scalar * C(1);
213 Product(2) = scalar * C(2);
214 Product(3) = scalar * C(3);
219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
221 double FGColumnVector3::Magnitude(void)
225 if ((data[1] == 0.00) &&
231 num = data[1]*data[1];
232 num += data[2]*data[2];
233 num += data[3]*data[3];
238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
240 FGColumnVector3 FGColumnVector3::Normalize(void)
242 double Mag = Magnitude();
254 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
256 FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V)
258 FGColumnVector3 Product;
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);
267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
269 void FGColumnVector3::operator*=(const FGColumnVector3& V)
272 a = data[1]; b=data[2]; c=data[3];
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);
280 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
282 FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V)
284 FGColumnVector3 Product;
286 Product(1) = data[1] * V(1);
287 Product(2) = data[2] * V(2);
288 Product(3) = data[3] * V(3);
293 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
295 void FGColumnVector3::Debug(void)
297 //TODO: Add your source code here
300 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
302 ostream& operator<<(ostream& os, const FGColumnVector3& col)
304 os << col(1) << " , " << col(2) << " , " << col(3);
308 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
310 FGColumnVector3& FGColumnVector3::operator<<(const double ff)