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 "FGMatrix33.h"
22 #include "FGColumnVector3.h"
24 static const char *IdSrc = "$Id$";
25 static const char *IdHdr = ID_MATRIX33;
27 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
31 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 FGMatrix33::FGMatrix33(void)
38 if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
41 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
43 FGMatrix33::FGMatrix33(int r, int c)
48 if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
51 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 FGMatrix33::FGMatrix33(const FGMatrix33& M)
61 data[1][1] = M.data[1][1];
62 data[1][2] = M.data[1][2];
63 data[1][3] = M.data[1][3];
64 data[2][1] = M.data[2][1];
65 data[2][2] = M.data[2][2];
66 data[2][3] = M.data[2][3];
67 data[3][1] = M.data[3][1];
68 data[3][2] = M.data[3][2];
69 data[3][3] = M.data[3][3];
71 if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
74 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
76 FGMatrix33::~FGMatrix33(void)
80 if (debug_lvl & 2) cout << "Destroyed: FGMatrix33" << endl;
83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
85 ostream& operator<<(ostream& os, const FGMatrix33& M)
87 for (unsigned int i=1; i<=M.Rows(); i++) {
88 for (unsigned int j=1; j<=M.Cols(); j++) {
89 if (i == M.Rows() && j == M.Cols())
92 os << M.data[i][j] << ", ";
98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
100 FGMatrix33& FGMatrix33::operator<<(const float ff)
102 data[rowCtr][colCtr] = ff;
103 if (++colCtr > Cols()) {
105 if (++rowCtr > Rows())
111 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
113 istream& operator>>(istream& is, FGMatrix33& M)
115 for (unsigned int i=1; i<=M.Rows(); i++) {
116 for (unsigned int j=1; j<=M.Cols(); j++) {
123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
125 FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
133 data[1][1] = M.data[1][1];
134 data[1][2] = M.data[1][2];
135 data[1][3] = M.data[1][3];
136 data[2][1] = M.data[2][1];
137 data[2][2] = M.data[2][2];
138 data[2][3] = M.data[2][3];
139 data[3][1] = M.data[3][1];
140 data[3][2] = M.data[3][2];
141 data[3][3] = M.data[3][3];
147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
149 void FGMatrix33::SetOParams(char delim,int width,int prec,int origin)
151 FGMatrix33::delim = delim;
152 FGMatrix33::width = width;
153 FGMatrix33::prec = prec;
154 FGMatrix33::origin = origin;
157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
159 void FGMatrix33::InitMatrix(double value)
174 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
176 void FGMatrix33::InitMatrix(void)
181 // *****************************************************************************
182 // binary operators ************************************************************
183 // *****************************************************************************
185 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
189 Diff(1,1) = data[1][1] - M(1,1);
190 Diff(1,2) = data[1][2] - M(1,2);
191 Diff(1,3) = data[1][3] - M(1,3);
192 Diff(2,1) = data[2][1] - M(2,1);
193 Diff(2,2) = data[2][2] - M(2,2);
194 Diff(2,3) = data[2][3] - M(2,3);
195 Diff(3,1) = data[3][1] - M(3,1);
196 Diff(3,2) = data[3][2] - M(3,2);
197 Diff(3,3) = data[3][3] - M(3,3);
203 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
205 void FGMatrix33::operator-=(const FGMatrix33 &M)
207 data[1][1] -= M(1,1);
208 data[1][2] -= M(1,2);
209 data[1][3] -= M(1,3);
210 data[2][1] -= M(2,1);
211 data[2][2] -= M(2,2);
212 data[2][3] -= M(2,3);
213 data[3][1] -= M(3,1);
214 data[3][2] -= M(3,2);
215 data[3][3] -= M(3,3);
219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
221 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
225 Sum(1,1) = data[1][1] + M(1,1);
226 Sum(1,2) = data[1][2] + M(1,2);
227 Sum(1,3) = data[1][3] + M(1,3);
228 Sum(2,1) = data[2][1] + M(2,1);
229 Sum(2,2) = data[2][2] + M(2,2);
230 Sum(2,3) = data[2][3] + M(2,3);
231 Sum(3,1) = data[3][1] + M(3,1);
232 Sum(3,2) = data[3][2] + M(3,2);
233 Sum(3,3) = data[3][3] + M(3,3);
238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
240 void FGMatrix33::operator+=(const FGMatrix33 &M)
242 data[1][1] += M(1,1);
243 data[1][2] += M(1,2);
244 data[1][3] += M(1,3);
245 data[2][1] += M(2,1);
246 data[2][2] += M(2,2);
247 data[2][3] += M(2,3);
248 data[3][1] += M(3,1);
249 data[3][2] += M(3,2);
250 data[3][3] += M(3,3);
254 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
256 FGMatrix33 FGMatrix33::operator*(const double scalar)
260 Product(1,1) = data[1][1] * scalar;
261 Product(1,2) = data[1][2] * scalar;
262 Product(1,3) = data[1][3] * scalar;
263 Product(2,1) = data[2][1] * scalar;
264 Product(2,2) = data[2][2] * scalar;
265 Product(2,3) = data[2][3] * scalar;
266 Product(3,1) = data[3][1] * scalar;
267 Product(3,2) = data[3][2] * scalar;
268 Product(3,3) = data[3][3] * scalar;
273 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
275 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
279 Product(1,1) = M(1,1) * scalar;
280 Product(1,2) = M(1,2) * scalar;
281 Product(1,3) = M(1,3) * scalar;
282 Product(2,1) = M(2,1) * scalar;
283 Product(2,2) = M(2,2) * scalar;
284 Product(2,3) = M(2,3) * scalar;
285 Product(3,1) = M(3,1) * scalar;
286 Product(3,2) = M(3,2) * scalar;
287 Product(3,3) = M(3,3) * scalar;
292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
294 void FGMatrix33::operator*=(const double scalar)
297 data[1][1] *= scalar;
298 data[1][2] *= scalar;
299 data[1][3] *= scalar;
300 data[2][1] *= scalar;
301 data[2][2] *= scalar;
302 data[2][3] *= scalar;
303 data[3][1] *= scalar;
304 data[3][2] *= scalar;
305 data[3][3] *= scalar;
309 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
311 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
315 Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
316 Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
317 Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
318 Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
319 Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
320 Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
321 Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
322 Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
323 Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330 void FGMatrix33::operator*=(const FGMatrix33& M)
334 a = data[1][1]; b=data[1][2]; c=data[1][3];
335 data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
336 data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
337 data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
339 a = data[2][1]; b=data[2][2]; c=data[2][3];
340 data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
341 data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
342 data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
344 a = data[3][1]; b=data[3][2]; c=data[3][3];
345 data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
346 data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
347 data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
353 FGMatrix33 FGMatrix33::operator/(const double scalar)
358 double tmp = 1.0/scalar;
359 Quot(1,1) = data[1][1] * tmp;
360 Quot(1,2) = data[1][2] * tmp;
361 Quot(1,3) = data[1][3] * tmp;
362 Quot(2,1) = data[2][1] * tmp;
363 Quot(2,2) = data[2][2] * tmp;
364 Quot(2,3) = data[2][3] * tmp;
365 Quot(3,1) = data[3][1] * tmp;
366 Quot(3,2) = data[3][2] * tmp;
367 Quot(3,3) = data[3][3] * tmp;
370 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
376 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
378 void FGMatrix33::operator/=(const double scalar)
381 double tmp = 1.0/scalar;
393 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
398 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
400 void FGMatrix33::T(void)
402 for (unsigned int i=1; i<=3; i++) {
403 for (unsigned int j=i+1; j<=3; j++) {
404 double tmp = data[i][j];
405 data[i][j] = data[j][i];
411 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
413 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
415 FGColumnVector3 Product;
417 Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
418 Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
419 Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
424 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
426 void FGMatrix33::Debug(void)
428 //TODO: Add your source code here
431 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%