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 /*******************************************************************************
24 ************************************ CODE **************************************
25 *******************************************************************************/
27 double** FGalloc(int rows, int cols)
31 A = new double *[rows+1];
34 for (int i=0; i <= rows; i++){
35 A[i] = new double [cols+1];
36 if (!A[i]) return NULL;
41 /******************************************************************************/
43 void dealloc(double **A, int rows)
45 for(int i=0;i<= rows;i++) delete[] A[i];
49 /******************************************************************************/
51 FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
53 data = FGalloc(rows,cols);
58 /******************************************************************************/
60 FGMatrix::FGMatrix(const FGMatrix& M)
67 /******************************************************************************/
69 FGMatrix::~FGMatrix(void)
76 /******************************************************************************/
78 ostream& operator<<(ostream& os, const FGMatrix& M)
80 for (unsigned int i=1; i<=M.Rows(); i++) {
81 for (unsigned int j=1; j<=M.Cols(); j++) {
82 if (i == M.Rows() && j == M.Cols())
85 os << M.data[i][j] << ", ";
91 /******************************************************************************/
93 FGMatrix& FGMatrix::operator<<(const float ff)
95 data[rowCtr][colCtr] = ff;
96 if (++colCtr > Cols()) {
98 if (++rowCtr > Rows())
104 /******************************************************************************/
106 istream& operator>>(istream& is, FGMatrix& M)
108 for (unsigned int i=1; i<=M.Rows(); i++) {
109 for (unsigned int j=1; j<=M.Cols(); j++) {
116 /******************************************************************************/
118 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
121 if (data != NULL) dealloc(data,rows);
130 data = FGalloc(rows,cols);
131 for (unsigned int i=0; i<=rows; i++) {
132 for (unsigned int j=0; j<=cols; j++) {
133 data[i][j] = M.data[i][j];
140 /******************************************************************************/
142 unsigned int FGMatrix::Rows(void) const
147 /******************************************************************************/
149 unsigned int FGMatrix::Cols(void) const
154 /******************************************************************************/
156 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
158 FGMatrix::delim = delim;
159 FGMatrix::width = width;
160 FGMatrix::prec = prec;
161 FGMatrix::origin = origin;
164 /******************************************************************************/
166 void FGMatrix::InitMatrix(double value)
169 for (unsigned int i=0;i<=rows;i++) {
170 for (unsigned int j=0;j<=cols;j++) {
171 operator()(i,j) = value;
177 /******************************************************************************/
179 void FGMatrix::InitMatrix(void)
184 // *****************************************************************************
185 // binary operators ************************************************************
186 // *****************************************************************************
188 FGMatrix FGMatrix::operator-(const FGMatrix& M)
190 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
192 mE.Message = "Invalid row/column match in Matrix operator -";
196 FGMatrix Diff(Rows(), Cols());
198 for (unsigned int i=1; i<=Rows(); i++) {
199 for (unsigned int j=1; j<=Cols(); j++) {
200 Diff(i,j) = data[i][j] - M(i,j);
206 /******************************************************************************/
208 void FGMatrix::operator-=(const FGMatrix &M)
210 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
212 mE.Message = "Invalid row/column match in Matrix operator -=";
216 for (unsigned int i=1; i<=Rows(); i++) {
217 for (unsigned int j=1; j<=Cols(); j++) {
218 data[i][j] -= M(i,j);
223 /******************************************************************************/
225 FGMatrix FGMatrix::operator+(const FGMatrix& M)
227 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
229 mE.Message = "Invalid row/column match in Matrix operator +";
233 FGMatrix Sum(Rows(), Cols());
235 for (unsigned int i=1; i<=Rows(); i++) {
236 for (unsigned int j=1; j<=Cols(); j++) {
237 Sum(i,j) = data[i][j] + M(i,j);
243 /******************************************************************************/
245 void FGMatrix::operator+=(const FGMatrix &M)
247 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
249 mE.Message = "Invalid row/column match in Matrix operator +=";
253 for (unsigned int i=1; i<=Rows(); i++) {
254 for (unsigned int j=1; j<=Cols(); j++) {
260 /******************************************************************************/
262 FGMatrix operator*(double scalar, FGMatrix &M)
264 FGMatrix Product(M.Rows(), M.Cols());
266 for (unsigned int i=1; i<=M.Rows(); i++) {
267 for (unsigned int j=1; j<=M.Cols(); j++) {
268 Product(i,j) = scalar*M(i,j);
274 /******************************************************************************/
276 void FGMatrix::operator*=(const double scalar)
278 for (unsigned int i=1; i<=Rows(); i++) {
279 for (unsigned int j=1; j<=Cols(); j++) {
280 data[i][j] *= scalar;
285 /******************************************************************************/
287 FGMatrix FGMatrix::operator*(const FGMatrix& M)
289 if (Cols() != M.Rows()) {
291 mE.Message = "Invalid row/column match in Matrix operator *";
295 FGMatrix Product(Rows(), M.Cols());
297 for (unsigned int i=1; i<=Rows(); i++) {
298 for (unsigned int j=1; j<=M.Cols(); j++) {
300 for (unsigned int k=1; k<=Cols(); k++) {
301 Product(i,j) += data[i][k] * M(k,j);
308 /******************************************************************************/
310 void FGMatrix::operator*=(const FGMatrix& M)
312 if (Cols() != M.Rows()) {
314 mE.Message = "Invalid row/column match in Matrix operator *=";
320 prod = FGalloc(Rows(), M.Cols());
321 for (unsigned int i=1; i<=Rows(); i++) {
322 for (unsigned int j=1; j<=M.Cols(); j++) {
324 for (unsigned int k=1; k<=Cols(); k++) {
325 prod[i][j] += data[i][k] * M(k,j);
329 dealloc(data, Rows());
334 /******************************************************************************/
336 FGMatrix FGMatrix::operator/(const double scalar)
338 FGMatrix Quot(Rows(), Cols());
341 for (unsigned int i=1; i<=Rows(); i++) {
342 for (unsigned int j=1; j<=Cols(); j++) {
343 Quot(i,j) = data[i][j]/scalar;
348 cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl;
352 /******************************************************************************/
354 void FGMatrix::operator/=(const double scalar)
358 for (unsigned int i=1; i<=Rows(); i++) {
359 for (unsigned int j=1; j<=Cols(); j++) {
364 cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl;
367 /******************************************************************************/
369 void FGMatrix::T(void)
374 TransposeNonSquare();
377 /******************************************************************************/
379 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
381 FGColumnVector Product(Col.Rows());
383 if (Cols() != Col.Rows()) {
385 mE.Message = "Invalid row/column match in Column Vector operator *";
389 for (unsigned int i=1;i<=Rows();i++) {
391 for (unsigned int j=1;j<=Cols();j++) {
392 Product(i) += Col(j)*data[i][j];
398 /******************************************************************************/
400 void FGMatrix::TransposeSquare(void)
402 for (unsigned int i=1; i<=rows; i++) {
403 for (unsigned int j=i+1; j<=cols; j++) {
404 double tmp = data[i][j];
405 data[i][j] = data[j][i];
411 /******************************************************************************/
413 void FGMatrix::TransposeNonSquare(void)
417 tran = FGalloc(rows,cols);
419 for (unsigned int i=1; i<=rows; i++) {
420 for (unsigned int j=1; j<=cols; j++) {
421 tran[j][i] = data[i][j];
428 unsigned int m = rows;
433 /******************************************************************************/
435 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
436 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
437 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
438 FGColumnVector::~FGColumnVector() { }
440 /******************************************************************************/
442 double& FGColumnVector::operator()(int m) const
444 return FGMatrix::operator()(m,1);
447 /******************************************************************************/
449 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
451 FGColumnVector Product(Col.Rows());
453 if (Mat.Cols() != Col.Rows()) {
455 mE.Message = "Invalid row/column match in Column Vector operator *";
459 for (unsigned int i=1;i<=Mat.Rows();i++) {
461 for (unsigned int j=1;j<=Mat.Cols();j++) {
462 Product(i) += Col(j)*Mat(i,j);
469 /******************************************************************************/
471 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
473 if (Rows() != C.Rows()) {
475 mE.Message = "Invalid row/column match in Column Vector operator *";
479 FGColumnVector Sum(C.Rows());
481 for (unsigned int i=1; i<=C.Rows(); i++) {
482 Sum(i) = C(i) + data[i][1];
488 /******************************************************************************/
490 FGColumnVector FGColumnVector::operator*(const double scalar)
492 FGColumnVector Product(Rows());
494 for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
499 /******************************************************************************/
501 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
503 if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
505 mE.Message = "Invalid row/column match in Column Vector operator -";
509 FGColumnVector Diff(Rows());
511 for (unsigned int i=1; i<=Rows(); i++) {
512 Diff(i) = data[i][1] - V(i);
518 /******************************************************************************/
520 FGColumnVector FGColumnVector::operator/(const double scalar)
522 FGColumnVector Quotient(Rows());
526 for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
529 cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl;
535 /******************************************************************************/
537 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
539 FGColumnVector Product(C.Rows());
541 for (unsigned int i=1; i<=C.Rows(); i++) {
542 Product(i) = scalar * C(i);
548 /******************************************************************************/
550 float FGColumnVector::Magnitude(void)
554 if ((data[1][1] == 0.00) &&
555 (data[2][1] == 0.00) &&
556 (data[3][1] == 0.00))
560 for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
565 /******************************************************************************/
567 FGColumnVector FGColumnVector::Normalize(void)
569 double Mag = Magnitude();
572 for (unsigned int i=1; i<=Rows(); i++)
573 for (unsigned int j=1; j<=Cols(); j++)
574 data[i][j] = data[i][j]/Mag;
580 /******************************************************************************/
582 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
584 if (Rows() != 3 || V.Rows() != 3) {
586 mE.Message = "Invalid row count in vector cross product function";
590 FGColumnVector Product(3);
592 Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
593 Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
594 Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
599 /******************************************************************************/
601 FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
603 if (Rows() != 3 || V.Rows() != 3) {
605 mE.Message = "Invalid row count in vector cross product function";
609 FGColumnVector Product(3);
611 Product(1) = data[1][1] * V(1);
612 Product(2) = data[2][1] * V(2);
613 Product(3) = data[3][1] * V(3);
618 /******************************************************************************/