FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
{
data = FGalloc(rows,cols);
+ InitMatrix();
rowCtr = colCtr = 1;
}
for (unsigned int i=1; i<=Rows(); i++) {
for (unsigned int j=1; j<=Cols(); j++) {
- Diff(i,j) = (*this)(i,j) - M(i,j);
+ Diff(i,j) = data[i][j] - M(i,j);
}
}
return Diff;
for (unsigned int i=1; i<=Rows(); i++) {
for (unsigned int j=1; j<=Cols(); j++) {
- (*this)(i,j) -= M(i,j);
+ data[i][j] -= M(i,j);
}
}
}
for (unsigned int i=1; i<=Rows(); i++) {
for (unsigned int j=1; j<=Cols(); j++) {
- Sum(i,j) = (*this)(i,j) + M(i,j);
+ Sum(i,j) = data[i][j] + M(i,j);
}
}
return Sum;
for (unsigned int i=1; i<=Rows(); i++) {
for (unsigned int j=1; j<=Cols(); j++) {
- (*this)(i,j)+=M(i,j);
+ data[i][j]+=M(i,j);
}
}
}
{
for (unsigned int i=1; i<=Rows(); i++) {
for (unsigned int j=1; j<=Cols(); j++) {
- (*this)(i,j) *= scalar;
+ data[i][j] *= scalar;
}
}
}
for (unsigned int j=1; j<=M.Cols(); j++) {
Product(i,j) = 0;
for (unsigned int k=1; k<=Cols(); k++) {
- Product(i,j) += (*this)(i,k) * M(k,j);
+ Product(i,j) += data[i][k] * M(k,j);
}
}
}
for (unsigned int j=1; j<=M.Cols(); j++) {
prod[i][j] = 0;
for (unsigned int k=1; k<=Cols(); k++) {
- prod[i][j] += (*this)(i,k) * M(k,j);
+ prod[i][j] += data[i][k] * M(k,j);
}
}
}
{
FGMatrix Quot(Rows(), Cols());
- for (unsigned int i=1; i<=Rows(); i++) {
- for (unsigned int j=1; j<=Cols(); j++) {
- Quot(i,j) = (*this)(i,j)/scalar;
+ if(scalar != 0) {
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ Quot(i,j) = data[i][j]/scalar;
+ }
}
- }
- return Quot;
+
+ } else
+ cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl;
+ return Quot;
}
/******************************************************************************/
void FGMatrix::operator/=(const double scalar)
{
- for (unsigned int i=1; i<=Rows(); i++) {
- for (unsigned int j=1; j<=Cols(); j++) {
- (*this)(i,j)/=scalar;
+
+ if(scalar != 0) {
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ data[i][j]/=scalar;
+ }
}
- }
+ } else
+ cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl;
}
/******************************************************************************/
/******************************************************************************/
+FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
+{
+ if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Column Vector operator -";
+ throw mE;
+ }
+
+ FGColumnVector Diff(Rows());
+
+ for (unsigned int i=1; i<=Rows(); i++) {
+ Diff(i) = data[i][1] - V(i);
+ }
+
+ return Diff;
+}
+
+/******************************************************************************/
+
FGColumnVector FGColumnVector::operator/(const double scalar)
{
FGColumnVector Quotient(Rows());
+ if(scalar != 0) {
+
- for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
+ for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
+ } else
+ cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl;
return Quotient;
+
+
}
/******************************************************************************/
}
/******************************************************************************/
+
float FGColumnVector::Magnitude(void)
{
double num=0.0;
FGColumnVector FGColumnVector::Normalize(void)
{
- return *this/Magnitude();
+ double Mag = Magnitude();
+
+ if (Mag != 0) {
+ for (unsigned int i=1; i<=Rows(); i++)
+ for (unsigned int j=1; j<=Cols(); j++)
+ data[i][j] = data[i][j]/Mag;
+ }
+
+ return *this;
}
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
+{
+ if (Rows() != 3 || V.Rows() != 3) {
+ MatrixException mE;
+ mE.Message = "Invalid row count in vector cross product function";
+ throw mE;
+ }
+
+ FGColumnVector Product(3);
+
+ Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
+ Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
+ Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
+
+ return Product;
+}
+
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
+{
+ if (Rows() != 3 || V.Rows() != 3) {
+ MatrixException mE;
+ mE.Message = "Invalid row count in vector cross product function";
+ throw mE;
+ }
+
+ FGColumnVector Product(3);
+
+ Product(1) = data[1][1] * V(1);
+ Product(2) = data[2][1] * V(2);
+ Product(3) = data[3][1] * V(3);
+
+ return Product;
+}
+
+/******************************************************************************/