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 "FGColumnVector4.h"
23 static const char *IdSrc = "$Id$";
24 static const char *IdHdr = ID_COLUMNVECTOR4;
26 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
28 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
30 FGColumnVector4::FGColumnVector4(void)
33 //cout << "Allocated: " << data << endl;
34 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
37 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 FGColumnVector4::FGColumnVector4(int m)
42 data[1]=0;data[2]=0;data[3]=0;data[4]=0;
43 //cout << "Allocated: " << data << endl;
44 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
47 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 FGColumnVector4::~FGColumnVector4(void)
51 //cout << "Freed: " << data << endl;
52 if (debug_lvl & 2) cout << "Destroyed: FGColumnVector4" << endl;
56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 FGColumnVector4::FGColumnVector4(const FGColumnVector4& b)
67 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72 FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b)
80 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
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];
98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
100 void FGColumnVector4::operator+=(const FGColumnVector4& C)
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 FGColumnVector4 FGColumnVector4::operator*(const double scalar)
112 FGColumnVector4 Product;
114 Product(1) = scalar * data[1];
115 Product(2) = scalar * data[2];
116 Product(3) = scalar * data[3];
117 Product(4) = scalar * data[4];
122 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
124 void FGColumnVector4::operator*=(const double scalar)
132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
134 FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
137 FGColumnVector4 Diff;
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);
147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
149 void FGColumnVector4::operator-=(const FGColumnVector4& V)
157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
159 FGColumnVector4 FGColumnVector4::operator/(const double scalar)
161 FGColumnVector4 Quotient;
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;
170 cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
177 void FGColumnVector4::operator/=(const double scalar)
179 FGColumnVector4 Quotient;
182 double tmp = 1.0/scalar;
188 cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
194 FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
196 FGColumnVector4 Product;
198 Product(1) = scalar * C(1);
199 Product(2) = scalar * C(2);
200 Product(3) = scalar * C(3);
201 Product(4) = scalar * C(4);
205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
207 float FGColumnVector4::Magnitude(void)
211 if ((data[1] == 0.00) &&
218 num = data[1]*data[1];
219 num += data[2]*data[2];
220 num += data[3]*data[3];
221 num += data[4]*data[4];
226 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
228 FGColumnVector4 FGColumnVector4::Normalize(void)
230 double Mag = Magnitude();
243 /* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
245 FGColumnVector4 FGColumnVector4::operator*(const FGColumnVector4& V)
247 FGColumnVector4 Product;
249 Product(1) = data[2] * V(3) - data[3] * V(2);
250 Product(2) = data[3] * V(1) - data[1] * V(3);
251 Product(3) = data[1] * V(2) - data[2] * V(1);
257 /* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
259 void FGColumnVector4::operator*=(const FGColumnVector4& V)
262 a = data[1]; b=data[2]; c=data[3];
264 data[1] = b * V(3) - c * V(2);
265 data[2] = c * V(1) - a * V(3);
266 data[3] = a * V(2) - b * V(1);
271 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
273 FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
275 FGColumnVector4 Product;
277 Product(1) = data[1] * V(1);
278 Product(2) = data[2] * V(2);
279 Product(3) = data[3] * V(3);
280 Product(4) = data[4] * V(4);
285 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
287 void FGColumnVector4::Debug(void)
289 //TODO: Add your source code here
292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
294 ostream& operator<<(ostream& os, FGColumnVector4& col)
296 os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
300 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
302 FGColumnVector4& FGColumnVector4::operator<<(const float ff)