1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
6 Purpose: FGMatrix class
10 --------------------------------------------------------------------------------
13 --------------------------------------------------------------------------------
15 03/16/2000 JSB Added exception throwing
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
23 static const char *IdSrc = "$Id$";
24 static const char *IdHdr = ID_MATRIX;
26 extern short debug_lvl;
28 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
32 double** FGalloc(int rows, int cols)
36 A = new double *[rows+1];
39 for (int i=0; i <= rows; i++){
40 A[i] = new double [cols+1];
41 if (!A[i]) return NULL;
46 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
48 void dealloc(double **A, int rows)
50 for (int i=0; i <= rows; i++) delete[] A[i];
54 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
58 data = FGalloc(rows,cols);
62 if (debug_lvl & 2) cout << "Instantiated: FGMatrix" << endl;
65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
67 FGMatrix::FGMatrix(const FGMatrix& M)
73 if (debug_lvl & 2) cout << "Instantiated: FGMatrix" << endl;
76 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 FGMatrix::~FGMatrix(void)
84 if (debug_lvl & 2) cout << "Destroyed: FGMatrix" << endl;
87 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
89 ostream& operator<<(ostream& os, const FGMatrix& M)
91 for (unsigned int i=1; i<=M.Rows(); i++) {
92 for (unsigned int j=1; j<=M.Cols(); j++) {
93 if (i == M.Rows() && j == M.Cols())
96 os << M.data[i][j] << ", ";
102 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
104 FGMatrix& FGMatrix::operator<<(const float ff)
106 data[rowCtr][colCtr] = ff;
107 if (++colCtr > Cols()) {
109 if (++rowCtr > Rows())
115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
117 istream& operator>>(istream& is, FGMatrix& M)
119 for (unsigned int i=1; i<=M.Rows(); i++) {
120 for (unsigned int j=1; j<=M.Cols(); j++) {
127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
132 if (M.rows != rows || M.cols != cols) {
133 if (data != NULL) dealloc(data,rows);
138 data = FGalloc(rows,cols);
140 for (unsigned int i=1; i<=rows; i++) {
141 for (unsigned int j=1; j<=cols; j++) {
142 data[i][j] = M.data[i][j];
149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
151 unsigned int FGMatrix::Rows(void) const
156 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
158 unsigned int FGMatrix::Cols(void) const
163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
165 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
169 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
171 void FGMatrix::InitMatrix(double value)
174 for (unsigned int i=0;i<=rows;i++) {
175 for (unsigned int j=0;j<=cols;j++) {
176 operator()(i,j) = value;
182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
184 void FGMatrix::InitMatrix(void)
189 // *****************************************************************************
190 // binary operators ************************************************************
191 // *****************************************************************************
193 FGMatrix FGMatrix::operator-(const FGMatrix& M)
195 FGMatrix Diff(Rows(), Cols());
197 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
199 mE.Message = "Invalid row/column match in Matrix operator -";
203 for (unsigned int i=1; i<=Rows(); i++) {
204 for (unsigned int j=1; j<=Cols(); j++) {
205 Diff(i,j) = data[i][j] - M(i,j);
211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
213 void FGMatrix::operator-=(const FGMatrix &M)
215 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
217 mE.Message = "Invalid row/column match in Matrix operator -=";
221 for (unsigned int i=1; i<=Rows(); i++) {
222 for (unsigned int j=1; j<=Cols(); j++) {
223 data[i][j] -= M(i,j);
228 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
230 FGMatrix FGMatrix::operator+(const FGMatrix& M)
232 FGMatrix Sum(Rows(), Cols());
234 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
236 mE.Message = "Invalid row/column match in Matrix operator +";
240 for (unsigned int i=1; i<=Rows(); i++) {
241 for (unsigned int j=1; j<=Cols(); j++) {
242 Sum(i,j) = data[i][j] + M(i,j);
248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
250 void FGMatrix::operator+=(const FGMatrix &M)
252 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
254 mE.Message = "Invalid row/column match in Matrix operator +=";
258 for (unsigned int i=1; i<=Rows(); i++) {
259 for (unsigned int j=1; j<=Cols(); j++) {
265 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
267 FGMatrix operator*(double scalar, FGMatrix &M)
269 FGMatrix Product(M.Rows(), M.Cols());
271 for (unsigned int i=1; i<=M.Rows(); i++) {
272 for (unsigned int j=1; j<=M.Cols(); j++) {
273 Product(i,j) = scalar*M(i,j);
279 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
281 void FGMatrix::operator*=(const double scalar)
283 for (unsigned int i=1; i<=Rows(); i++) {
284 for (unsigned int j=1; j<=Cols(); j++) {
285 data[i][j] *= scalar;
290 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
292 FGMatrix FGMatrix::operator*(const FGMatrix& M)
294 FGMatrix Product(Rows(), M.Cols());
296 if (Cols() != M.Rows()) {
298 mE.Message = "Invalid row/column match in Matrix operator *";
302 for (unsigned int i=1; i<=Rows(); i++) {
303 for (unsigned int j=1; j<=M.Cols(); j++) {
305 for (unsigned int k=1; k<=Cols(); k++) {
306 Product(i,j) += data[i][k] * M(k,j);
313 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
315 void FGMatrix::operator*=(const FGMatrix& M)
317 if (Cols() != M.Rows()) {
319 mE.Message = "Invalid row/column match in Matrix operator *=";
325 prod = FGalloc(Rows(), M.Cols());
326 for (unsigned int i=1; i<=Rows(); i++) {
327 for (unsigned int j=1; j<=M.Cols(); j++) {
329 for (unsigned int k=1; k<=Cols(); k++) {
330 prod[i][j] += data[i][k] * M(k,j);
334 dealloc(data, Rows());
339 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
341 FGMatrix FGMatrix::operator/(const double scalar)
343 FGMatrix Quot(Rows(), Cols());
346 for (unsigned int i=1; i<=Rows(); i++) {
347 for (unsigned int j=1; j<=Cols(); j++) {
348 Quot(i,j) = data[i][j]/scalar;
353 cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl;
358 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
360 void FGMatrix::operator/=(const double scalar)
364 for (unsigned int i=1; i<=Rows(); i++) {
365 for (unsigned int j=1; j<=Cols(); j++) {
370 cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl;
373 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
375 void FGMatrix::T(void)
380 TransposeNonSquare();
383 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
385 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
387 FGColumnVector Product(Col.Rows());
389 if (Cols() != Col.Rows()) {
391 mE.Message = "Invalid row/column match in Column Vector operator *";
395 for (unsigned int i=1;i<=Rows();i++) {
397 for (unsigned int j=1;j<=Cols();j++) {
398 Product(i) += Col(j)*data[i][j];
404 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
406 void FGMatrix::TransposeSquare(void)
408 for (unsigned int i=1; i<=rows; i++) {
409 for (unsigned int j=i+1; j<=cols; j++) {
410 double tmp = data[i][j];
411 data[i][j] = data[j][i];
417 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
419 void FGMatrix::TransposeNonSquare(void)
423 tran = FGalloc(rows,cols);
425 for (unsigned int i=1; i<=rows; i++) {
426 for (unsigned int j=1; j<=cols; j++) {
427 tran[j][i] = data[i][j];
434 unsigned int m = rows;
439 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
441 void FGMatrix::Debug(void)
443 //TODO: Add your source code here
446 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
448 FGColumnVector::FGColumnVector(void):FGMatrix(3,1)
450 if (debug_lvl & 2) cout << "Instantiated: FGColumnVector" << endl;
453 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
454 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
456 FGColumnVector::~FGColumnVector(void)
458 // dealloc(data,rows);
459 if (debug_lvl & 2) cout << "Destroyed: FGColumnVector" << endl;
462 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
464 double& FGColumnVector::operator()(int m) const
466 return FGMatrix::operator()(m,1);
469 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
471 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
473 FGColumnVector Product(Col.Rows());
475 if (Mat.Cols() != Col.Rows()) {
477 mE.Message = "Invalid row/column match in Column Vector operator *";
481 for (unsigned int i=1;i<=Mat.Rows();i++) {
483 for (unsigned int j=1;j<=Mat.Cols();j++) {
484 Product(i) += Col(j)*Mat(i,j);
491 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
493 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
495 FGColumnVector Sum(C.Rows()); // This must be created dynamically
496 // because we don't know the size of "C",
497 // it could be 3 or 4 or ...
498 if (Rows() != C.Rows()) {
500 mE.Message = "Invalid row/column match in Column Vector operator *";
504 for (unsigned int i=1; i<=C.Rows(); i++) Sum(i) = C(i) + data[i][1];
509 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
511 FGColumnVector FGColumnVector::operator*(const double scalar)
513 FGColumnVector Product(Rows());
515 for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
520 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
522 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
524 FGColumnVector Diff(Rows());
526 if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
528 mE.Message = "Invalid row/column match in Column Vector operator -";
532 for (unsigned int i=1; i<=Rows(); i++) {
533 Diff(i) = data[i][1] - V(i);
539 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
541 FGColumnVector FGColumnVector::operator/(const double scalar)
543 FGColumnVector Quotient(Rows());
546 for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
548 cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl;
553 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
555 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
557 FGColumnVector Product(C.Rows());
559 for (unsigned int i=1; i<=C.Rows(); i++) {
560 Product(i) = scalar * C(i);
566 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
568 float FGColumnVector::Magnitude(void)
572 if ((data[1][1] == 0.00) &&
573 (data[2][1] == 0.00) &&
574 (data[3][1] == 0.00))
578 for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
583 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
585 FGColumnVector FGColumnVector::Normalize(void)
587 double Mag = Magnitude();
590 for (unsigned int i=1; i<=Rows(); i++)
591 for (unsigned int j=1; j<=Cols(); j++)
592 data[i][j] = data[i][j]/Mag;
598 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
600 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
602 FGColumnVector Product(3);
604 if (Rows() != 3 || V.Rows() != 3) {
606 mE.Message = "Invalid row count in vector cross product function";
610 Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
611 Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
612 Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
617 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
619 FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
621 FGColumnVector Product(3);
623 if (Rows() != 3 || V.Rows() != 3) {
625 mE.Message = "Invalid row count in vector cross product function";
629 Product(1) = data[1][1] * V(1);
630 Product(2) = data[2][1] * V(2);
631 Product(3) = data[3][1] * V(3);
636 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
638 void FGColumnVector::Debug(void)
640 //TODO: Add your source code here
643 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%