40 #include "FGMatrix33.h" 41 #include "FGColumnVector3.h" 42 #include "FGQuaternion.h" 52 IDENT(IdSrc,
"$Id: FGMatrix33.cpp,v 1.16 2014/01/13 10:46:03 ehofman Exp $");
53 IDENT(IdHdr,ID_MATRIX33);
61 FGMatrix33::FGMatrix33(
void)
63 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
64 data[6] = data[7] = data[8] = 0.0;
69 string FGMatrix33::Dump(
const string& delimiter)
const 72 buffer << setw(12) << setprecision(10) << data[0] << delimiter;
73 buffer << setw(12) << setprecision(10) << data[3] << delimiter;
74 buffer << setw(12) << setprecision(10) << data[6] << delimiter;
75 buffer << setw(12) << setprecision(10) << data[1] << delimiter;
76 buffer << setw(12) << setprecision(10) << data[4] << delimiter;
77 buffer << setw(12) << setprecision(10) << data[7] << delimiter;
78 buffer << setw(12) << setprecision(10) << data[2] << delimiter;
79 buffer << setw(12) << setprecision(10) << data[5] << delimiter;
80 buffer << setw(12) << setprecision(10) << data[8];
86 string FGMatrix33::Dump(
const string& delimiter,
const string& prefix)
const 90 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
91 buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
92 buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
94 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
95 buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
96 buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
98 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
99 buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
100 buffer << right << fixed << setw(9) << setprecision(6) << data[8];
102 buffer << setw(0) << left;
116 tempQ[0] = 1.0 + data[0] + data[4] + data[8];
117 tempQ[1] = 1.0 + data[0] - data[4] - data[8];
118 tempQ[2] = 1.0 - data[0] + data[4] - data[8];
119 tempQ[3] = 1.0 - data[0] - data[4] + data[8];
123 for (
int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
127 Q(1) = 0.50*sqrt(tempQ[0]);
128 Q(2) = 0.25*(data[7] - data[5])/Q(1);
129 Q(3) = 0.25*(data[2] - data[6])/Q(1);
130 Q(4) = 0.25*(data[3] - data[1])/Q(1);
133 Q(2) = 0.50*sqrt(tempQ[1]);
134 Q(1) = 0.25*(data[7] - data[5])/Q(2);
135 Q(3) = 0.25*(data[3] + data[1])/Q(2);
136 Q(4) = 0.25*(data[2] + data[6])/Q(2);
139 Q(3) = 0.50*sqrt(tempQ[2]);
140 Q(1) = 0.25*(data[2] - data[6])/Q(3);
141 Q(2) = 0.25*(data[3] + data[1])/Q(3);
142 Q(4) = 0.25*(data[7] + data[5])/Q(3);
145 Q(4) = 0.50*sqrt(tempQ[3]);
146 Q(1) = 0.25*(data[3] - data[1])/Q(4);
147 Q(2) = 0.25*(data[6] + data[2])/Q(4);
148 Q(3) = 0.25*(data[7] + data[5])/Q(4);
167 mEulerAngles(1) = 0.5*M_PI;
169 mEulerAngles(1) = atan2(data[7], data[8]);
172 mEulerAngles(2) = 0.5*M_PI;
173 else if (1.0 < data[6])
174 mEulerAngles(2) = -0.5*M_PI;
176 mEulerAngles(2) = asin(-data[6]);
179 mEulerAngles(3) = 0.5*M_PI;
181 double psi = atan2(data[3], data[0]);
184 mEulerAngles(3) = psi;
192 ostream& operator<<(ostream& os,
const FGMatrix33& M)
194 for (
unsigned int i=1; i<=M.
Rows(); i++) {
195 for (
unsigned int j=1; j<=M.
Cols(); j++) {
199 os << M(i,j) <<
", ";
207 istream& operator>>(istream& is,
FGMatrix33& M)
209 for (
unsigned int i=1; i<=M.
Rows(); i++) {
210 for (
unsigned int j=1; j<=M.
Cols(); j++) {
219 double FGMatrix33::Determinant(
void)
const {
220 return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
221 + data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
222 - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
232 if (Determinant() != 0.0) {
233 double rdet = 1.0/Determinant();
235 double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
236 double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
237 double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
238 double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
239 double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
240 double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
241 double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
242 double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
243 double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
257 void FGMatrix33::InitMatrix(
void)
259 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
260 data[6] = data[7] = data[8] = 0.0;
277 data[8] - M.data[8] );
284 data[0] -= M.data[0];
285 data[1] -= M.data[1];
286 data[2] -= M.data[2];
287 data[3] -= M.data[3];
288 data[4] -= M.data[4];
289 data[5] -= M.data[5];
290 data[6] -= M.data[6];
291 data[7] -= M.data[7];
292 data[8] -= M.data[8];
309 data[8] + M.data[8] );
316 data[0] += M.data[0];
317 data[3] += M.data[3];
318 data[6] += M.data[6];
319 data[1] += M.data[1];
320 data[4] += M.data[4];
321 data[7] += M.data[7];
322 data[2] += M.data[2];
323 data[5] += M.data[5];
324 data[8] += M.data[8];
382 Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
383 Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
384 Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
385 Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
386 Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
387 Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
388 Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
389 Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
390 Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
402 a = data[0]; b=data[3]; c=data[6];
403 data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
404 data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
405 data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
407 a = data[1]; b=data[4]; c=data[7];
408 data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
409 data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
410 data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
412 a = data[2]; b=data[5]; c=data[8];
413 data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
414 data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
415 data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
427 double tmp = 1.0/scalar;
428 Quot.data[0] = data[0] * tmp;
429 Quot.data[3] = data[3] * tmp;
430 Quot.data[6] = data[6] * tmp;
431 Quot.data[1] = data[1] * tmp;
432 Quot.data[4] = data[4] * tmp;
433 Quot.data[7] = data[7] * tmp;
434 Quot.data[2] = data[2] * tmp;
435 Quot.data[5] = data[5] * tmp;
436 Quot.data[8] = data[8] * tmp;
439 mE.Message =
"Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
450 double tmp = 1.0/scalar;
462 mE.Message =
"Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
470 void FGMatrix33::T(
void)
495 double tmp1 = v1*data[0];
496 double tmp2 = v1*data[1];
497 double tmp3 = v1*data[2];
Models the Quaternion representation of rotations.
unsigned int Cols(void) const
Number of cloumns in the matrix.
This class implements a 3 element column vector.
unsigned int Rows(void) const
Number of rows in the matrix.
Handles matrix math operations.
Exception convenience class.