public static Matrix33 Transpose(Matrix33 orig)
{
var transposedMatrix = new Matrix33(orig);
transposedMatrix.Transpose();
return transposedMatrix;
}
// Compute the derived values if required ...
private void ComputeDerivedUnconditional()
{
// First normalize the 4-vector
double norm = Magnitude();
if (Math.Abs(norm) < EqualityTolerance)
{
return;
}
double rnorm = 1.0/norm;
double q1 = rnorm*Q1;
double q2 = rnorm*Q2;
double q3 = rnorm*Q3;
double q4 = rnorm*Q4;
// Now compute the transformation matrix.
double q1Q1 = q1*q1;
double q2Q2 = q2*q2;
double q3Q3 = q3*q3;
double q4Q4 = q4*q4;
double q1Q2 = q1*q2;
double q1Q3 = q1*q3;
double q1Q4 = q1*q4;
double q2Q3 = q2*q3;
double q2Q4 = q2*q4;
double q3Q4 = q3*q4;
mT[1, 1] = q1Q1 + q2Q2 - q3Q3 - q4Q4;
mT[1, 2] = 2.0*(q2Q3 + q1Q4);
mT[1, 3] = 2.0*(q2Q4 - q1Q3);
mT[2, 1] = 2.0*(q2Q3 - q1Q4);
mT[2, 2] = q1Q1 - q2Q2 + q3Q3 - q4Q4;
mT[2, 3] = 2.0*(q3Q4 + q1Q2);
mT[3, 1] = 2.0*(q2Q4 + q1Q3);
mT[3, 2] = 2.0*(q3Q4 - q1Q2);
mT[3, 3] = q1Q1 - q2Q2 - q3Q3 + q4Q4;
// Since this is an orthogonal matrix, the inverse is simply
// the transpose.
mTInv = mT;
mTInv.Transpose();
// Compute the Euler-angles
if (Math.Abs(mT[3, 3]) < EqualityTolerance)
mEulerAngles[(int) EulerAngles.ePhi] = 0.5*Math.PI;
else
mEulerAngles[(int) EulerAngles.ePhi] = Math.Atan2(mT[2, 3], mT[3, 3]);
if (mT[1, 3] < -1.0)
mEulerAngles[(int) EulerAngles.eTht] = 0.5*Math.PI;
else if (1.0 < mT[1, 3])
mEulerAngles[(int) EulerAngles.eTht] = -0.5*Math.PI;
else
mEulerAngles[(int) EulerAngles.eTht] = Math.Asin(-mT[1, 3]);
if (Math.Abs(mT[1, 1]) < EqualityTolerance)
mEulerAngles[(int) EulerAngles.ePsi] = 0.5*Math.PI;
else
{
double psi = Math.Atan2(mT[1, 2], mT[1, 1]);
if (psi < 0.0)
psi += 2*Math.PI;
mEulerAngles[(int) EulerAngles.ePsi] = psi;
}
// FIXME: may be one can compute those values easier ???
mEulerSines[(int) EulerAngles.ePhi] = Math.Sin(mEulerAngles[(int) EulerAngles.ePhi]);
// mEulerSines(eTht) = sin(mEulerAngles(eTht));
mEulerSines[(int) EulerAngles.eTht] = -mT[1, 3];
mEulerSines[(int) EulerAngles.ePsi] = Math.Sin(mEulerAngles[(int) EulerAngles.ePsi]);
mEulerCosines[(int) EulerAngles.ePhi] = Math.Cos(mEulerAngles[(int) EulerAngles.ePhi]);
mEulerCosines[(int) EulerAngles.eTht] = Math.Cos(mEulerAngles[(int) EulerAngles.eTht]);
mEulerCosines[(int) EulerAngles.ePsi] = Math.Cos(mEulerAngles[(int) EulerAngles.ePsi]);
}
public void The_current_matrix_should_be_transposed()
{
var actual = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
actual.Transpose();
Assert.AreEqual(actual.A11, 1.0);
Assert.AreEqual(actual.A12, 4.6);
Assert.AreEqual(actual.A13, -0.9);
Assert.AreEqual(actual.A21, -2.0);
Assert.AreEqual(actual.A22, 3.2);
Assert.AreEqual(actual.A23, 8.1);
Assert.AreEqual(actual.A31, -3.0);
Assert.AreEqual(actual.A32, -1.2);
Assert.AreEqual(actual.A33, 3.4);
}