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 = "$Header$";
24 static const char *IdHdr = ID_MATRIX;
26 /*******************************************************************************
27 ************************************ CODE **************************************
28 *******************************************************************************/
30 double** FGalloc(int rows, int cols)
34 A = new double *[rows+1];
37 for (int i=0; i <= rows; i++){
38 A[i] = new double [cols+1];
39 if (!A[i]) return NULL;
44 /******************************************************************************/
46 void dealloc(double **A, int rows)
48 for(int i=0;i<= rows;i++) delete[] A[i];
52 /******************************************************************************/
54 FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
56 data = FGalloc(rows,cols);
61 /******************************************************************************/
63 FGMatrix::FGMatrix(const FGMatrix& M)
70 /******************************************************************************/
72 FGMatrix::~FGMatrix(void)
79 /******************************************************************************/
81 ostream& operator<<(ostream& os, const FGMatrix& M)
83 for (unsigned int i=1; i<=M.Rows(); i++) {
84 for (unsigned int j=1; j<=M.Cols(); j++) {
85 if (i == M.Rows() && j == M.Cols())
88 os << M.data[i][j] << ", ";
94 /******************************************************************************/
96 FGMatrix& FGMatrix::operator<<(const float ff)
98 data[rowCtr][colCtr] = ff;
99 if (++colCtr > Cols()) {
101 if (++rowCtr > Rows())
107 /******************************************************************************/
109 istream& operator>>(istream& is, FGMatrix& M)
111 for (unsigned int i=1; i<=M.Rows(); i++) {
112 for (unsigned int j=1; j<=M.Cols(); j++) {
119 /******************************************************************************/
121 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
124 if (data != NULL) dealloc(data,rows);
133 data = FGalloc(rows,cols);
134 for (unsigned int i=0; i<=rows; i++) {
135 for (unsigned int j=0; j<=cols; j++) {
136 data[i][j] = M.data[i][j];
143 /******************************************************************************/
145 unsigned int FGMatrix::Rows(void) const
150 /******************************************************************************/
152 unsigned int FGMatrix::Cols(void) const
157 /******************************************************************************/
159 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
161 FGMatrix::delim = delim;
162 FGMatrix::width = width;
163 FGMatrix::prec = prec;
164 FGMatrix::origin = origin;
167 /******************************************************************************/
169 void FGMatrix::InitMatrix(double value)
172 for (unsigned int i=0;i<=rows;i++) {
173 for (unsigned int j=0;j<=cols;j++) {
174 operator()(i,j) = value;
180 /******************************************************************************/
182 void FGMatrix::InitMatrix(void)
187 // *****************************************************************************
188 // binary operators ************************************************************
189 // *****************************************************************************
191 FGMatrix FGMatrix::operator-(const FGMatrix& M)
193 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
195 mE.Message = "Invalid row/column match in Matrix operator -";
199 FGMatrix Diff(Rows(), Cols());
201 for (unsigned int i=1; i<=Rows(); i++) {
202 for (unsigned int j=1; j<=Cols(); j++) {
203 Diff(i,j) = data[i][j] - M(i,j);
209 /******************************************************************************/
211 void FGMatrix::operator-=(const FGMatrix &M)
213 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
215 mE.Message = "Invalid row/column match in Matrix operator -=";
219 for (unsigned int i=1; i<=Rows(); i++) {
220 for (unsigned int j=1; j<=Cols(); j++) {
221 data[i][j] -= M(i,j);
226 /******************************************************************************/
228 FGMatrix FGMatrix::operator+(const FGMatrix& M)
230 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
232 mE.Message = "Invalid row/column match in Matrix operator +";
236 FGMatrix Sum(Rows(), Cols());
238 for (unsigned int i=1; i<=Rows(); i++) {
239 for (unsigned int j=1; j<=Cols(); j++) {
240 Sum(i,j) = data[i][j] + M(i,j);
246 /******************************************************************************/
248 void FGMatrix::operator+=(const FGMatrix &M)
250 if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
252 mE.Message = "Invalid row/column match in Matrix operator +=";
256 for (unsigned int i=1; i<=Rows(); i++) {
257 for (unsigned int j=1; j<=Cols(); j++) {
263 /******************************************************************************/
265 FGMatrix operator*(double scalar, FGMatrix &M)
267 FGMatrix Product(M.Rows(), M.Cols());
269 for (unsigned int i=1; i<=M.Rows(); i++) {
270 for (unsigned int j=1; j<=M.Cols(); j++) {
271 Product(i,j) = scalar*M(i,j);
277 /******************************************************************************/
279 void FGMatrix::operator*=(const double scalar)
281 for (unsigned int i=1; i<=Rows(); i++) {
282 for (unsigned int j=1; j<=Cols(); j++) {
283 data[i][j] *= scalar;
288 /******************************************************************************/
290 FGMatrix FGMatrix::operator*(const FGMatrix& M)
292 if (Cols() != M.Rows()) {
294 mE.Message = "Invalid row/column match in Matrix operator *";
298 FGMatrix Product(Rows(), M.Cols());
300 for (unsigned int i=1; i<=Rows(); i++) {
301 for (unsigned int j=1; j<=M.Cols(); j++) {
303 for (unsigned int k=1; k<=Cols(); k++) {
304 Product(i,j) += data[i][k] * M(k,j);
311 /******************************************************************************/
313 void FGMatrix::operator*=(const FGMatrix& M)
315 if (Cols() != M.Rows()) {
317 mE.Message = "Invalid row/column match in Matrix operator *=";
323 prod = FGalloc(Rows(), M.Cols());
324 for (unsigned int i=1; i<=Rows(); i++) {
325 for (unsigned int j=1; j<=M.Cols(); j++) {
327 for (unsigned int k=1; k<=Cols(); k++) {
328 prod[i][j] += data[i][k] * M(k,j);
332 dealloc(data, Rows());
337 /******************************************************************************/
339 FGMatrix FGMatrix::operator/(const double scalar)
341 FGMatrix Quot(Rows(), Cols());
344 for (unsigned int i=1; i<=Rows(); i++) {
345 for (unsigned int j=1; j<=Cols(); j++) {
346 Quot(i,j) = data[i][j]/scalar;
351 cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl;
355 /******************************************************************************/
357 void FGMatrix::operator/=(const double scalar)
361 for (unsigned int i=1; i<=Rows(); i++) {
362 for (unsigned int j=1; j<=Cols(); j++) {
367 cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl;
370 /******************************************************************************/
372 void FGMatrix::T(void)
377 TransposeNonSquare();
380 /******************************************************************************/
382 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
384 FGColumnVector Product(Col.Rows());
386 if (Cols() != Col.Rows()) {
388 mE.Message = "Invalid row/column match in Column Vector operator *";
392 for (unsigned int i=1;i<=Rows();i++) {
394 for (unsigned int j=1;j<=Cols();j++) {
395 Product(i) += Col(j)*data[i][j];
401 /******************************************************************************/
403 void FGMatrix::TransposeSquare(void)
405 for (unsigned int i=1; i<=rows; i++) {
406 for (unsigned int j=i+1; j<=cols; j++) {
407 double tmp = data[i][j];
408 data[i][j] = data[j][i];
414 /******************************************************************************/
416 void FGMatrix::TransposeNonSquare(void)
420 tran = FGalloc(rows,cols);
422 for (unsigned int i=1; i<=rows; i++) {
423 for (unsigned int j=1; j<=cols; j++) {
424 tran[j][i] = data[i][j];
431 unsigned int m = rows;
436 /******************************************************************************/
438 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
439 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
440 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
441 FGColumnVector::~FGColumnVector() { }
443 /******************************************************************************/
445 double& FGColumnVector::operator()(int m) const
447 return FGMatrix::operator()(m,1);
450 /******************************************************************************/
452 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
454 FGColumnVector Product(Col.Rows());
456 if (Mat.Cols() != Col.Rows()) {
458 mE.Message = "Invalid row/column match in Column Vector operator *";
462 for (unsigned int i=1;i<=Mat.Rows();i++) {
464 for (unsigned int j=1;j<=Mat.Cols();j++) {
465 Product(i) += Col(j)*Mat(i,j);
472 /******************************************************************************/
474 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
476 if (Rows() != C.Rows()) {
478 mE.Message = "Invalid row/column match in Column Vector operator *";
482 FGColumnVector Sum(C.Rows());
484 for (unsigned int i=1; i<=C.Rows(); i++) {
485 Sum(i) = C(i) + data[i][1];
491 /******************************************************************************/
493 FGColumnVector FGColumnVector::operator*(const double scalar)
495 FGColumnVector Product(Rows());
497 for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
502 /******************************************************************************/
504 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
506 if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
508 mE.Message = "Invalid row/column match in Column Vector operator -";
512 FGColumnVector Diff(Rows());
514 for (unsigned int i=1; i<=Rows(); i++) {
515 Diff(i) = data[i][1] - V(i);
521 /******************************************************************************/
523 FGColumnVector FGColumnVector::operator/(const double scalar)
525 FGColumnVector Quotient(Rows());
529 for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
532 cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl;
538 /******************************************************************************/
540 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
542 FGColumnVector Product(C.Rows());
544 for (unsigned int i=1; i<=C.Rows(); i++) {
545 Product(i) = scalar * C(i);
551 /******************************************************************************/
553 float FGColumnVector::Magnitude(void)
557 if ((data[1][1] == 0.00) &&
558 (data[2][1] == 0.00) &&
559 (data[3][1] == 0.00))
563 for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
568 /******************************************************************************/
570 FGColumnVector FGColumnVector::Normalize(void)
572 double Mag = Magnitude();
575 for (unsigned int i=1; i<=Rows(); i++)
576 for (unsigned int j=1; j<=Cols(); j++)
577 data[i][j] = data[i][j]/Mag;
583 /******************************************************************************/
585 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
587 if (Rows() != 3 || V.Rows() != 3) {
589 mE.Message = "Invalid row count in vector cross product function";
593 FGColumnVector Product(3);
595 Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
596 Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
597 Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
602 /******************************************************************************/
604 FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
606 if (Rows() != 3 || V.Rows() != 3) {
608 mE.Message = "Invalid row count in vector cross product function";
612 FGColumnVector Product(3);
614 Product(1) = data[1][1] * V(1);
615 Product(2) = data[2][1] * V(2);
616 Product(3) = data[3][1] * V(3);
621 /******************************************************************************/