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) = (*this)(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 (*this)(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) = (*this)(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++) {
255 (*this)(i,j)+=M(i,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 (*this)(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) += (*this)(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] += (*this)(i,k) * M(k,j);
329 dealloc(data, Rows());
334 /******************************************************************************/
336 FGMatrix FGMatrix::operator/(const double scalar)
338 FGMatrix Quot(Rows(), Cols());
340 for (unsigned int i=1; i<=Rows(); i++) {
341 for (unsigned int j=1; j<=Cols(); j++) {
342 Quot(i,j) = (*this)(i,j)/scalar;
348 /******************************************************************************/
350 void FGMatrix::operator/=(const double scalar)
352 for (unsigned int i=1; i<=Rows(); i++) {
353 for (unsigned int j=1; j<=Cols(); j++) {
354 (*this)(i,j)/=scalar;
359 /******************************************************************************/
361 void FGMatrix::T(void)
366 TransposeNonSquare();
369 /******************************************************************************/
371 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
373 FGColumnVector Product(Col.Rows());
375 if (Cols() != Col.Rows()) {
377 mE.Message = "Invalid row/column match in Column Vector operator *";
381 for (unsigned int i=1;i<=Rows();i++) {
383 for (unsigned int j=1;j<=Cols();j++) {
384 Product(i) += Col(j)*data[i][j];
390 /******************************************************************************/
392 void FGMatrix::TransposeSquare(void)
394 for (unsigned int i=1; i<=rows; i++) {
395 for (unsigned int j=i+1; j<=cols; j++) {
396 double tmp = data[i][j];
397 data[i][j] = data[j][i];
403 /******************************************************************************/
405 void FGMatrix::TransposeNonSquare(void)
409 tran = FGalloc(rows,cols);
411 for (unsigned int i=1; i<=rows; i++) {
412 for (unsigned int j=1; j<=cols; j++) {
413 tran[j][i] = data[i][j];
420 unsigned int m = rows;
425 /******************************************************************************/
427 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
428 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
429 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
430 FGColumnVector::~FGColumnVector() { }
432 /******************************************************************************/
434 double& FGColumnVector::operator()(int m) const
436 return FGMatrix::operator()(m,1);
439 /******************************************************************************/
441 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
443 FGColumnVector Product(Col.Rows());
445 if (Mat.Cols() != Col.Rows()) {
447 mE.Message = "Invalid row/column match in Column Vector operator *";
451 for (unsigned int i=1;i<=Mat.Rows();i++) {
453 for (unsigned int j=1;j<=Mat.Cols();j++) {
454 Product(i) += Col(j)*Mat(i,j);
461 /******************************************************************************/
463 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
465 if (Rows() != C.Rows()) {
467 mE.Message = "Invalid row/column match in Column Vector operator *";
471 FGColumnVector Sum(C.Rows());
473 for (unsigned int i=1; i<=C.Rows(); i++) {
474 Sum(i) = C(i) + data[i][1];
480 /******************************************************************************/
482 FGColumnVector FGColumnVector::operator*(const double scalar)
484 FGColumnVector Product(Rows());
486 for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
491 /******************************************************************************/
493 FGColumnVector FGColumnVector::operator/(const double scalar)
495 FGColumnVector Quotient(Rows());
497 for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
502 /******************************************************************************/
504 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
506 FGColumnVector Product(C.Rows());
508 for (unsigned int i=1; i<=C.Rows(); i++) {
509 Product(i) = scalar * C(i);
515 /******************************************************************************/
516 float FGColumnVector::Magnitude(void)
520 if ((data[1][1] == 0.00) &&
521 (data[2][1] == 0.00) &&
522 (data[3][1] == 0.00))
526 for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
531 /******************************************************************************/
533 FGColumnVector FGColumnVector::Normalize(void)
535 return *this/Magnitude();