public Matrix33(Matrix33 orig)
{
A11 = orig.A11;
A12 = orig.A12;
A13 = orig.A13;
A21 = orig.A21;
A22 = orig.A22;
A23 = orig.A23;
A31 = orig.A31;
A32 = orig.A32;
A33 = orig.A33;
}
public void A_zero_matrix_should_be_created()
{
var testMatrix = new Matrix33();
Assert.AreEqual(testMatrix.A11, 0.0);
Assert.AreEqual(testMatrix.A12, 0.0);
Assert.AreEqual(testMatrix.A13, 0.0);
Assert.AreEqual(testMatrix.A21, 0.0);
Assert.AreEqual(testMatrix.A22, 0.0);
Assert.AreEqual(testMatrix.A23, 0.0);
Assert.AreEqual(testMatrix.A31, 0.0);
Assert.AreEqual(testMatrix.A32, 0.0);
Assert.AreEqual(testMatrix.A33, 0.0);
}
public void A_filled_matrix_should_be_created()
{
var testMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
Assert.AreEqual(testMatrix.A11, 1.0);
Assert.AreEqual(testMatrix.A12, -2.0);
Assert.AreEqual(testMatrix.A13, -3.0);
Assert.AreEqual(testMatrix.A21, 4.6);
Assert.AreEqual(testMatrix.A22, 3.2);
Assert.AreEqual(testMatrix.A23, -1.2);
Assert.AreEqual(testMatrix.A31, -0.9);
Assert.AreEqual(testMatrix.A32, 8.1);
Assert.AreEqual(testMatrix.A33, 3.4);
}
public void A_new_matrix_should_be_created_from_another_one()
{
var copyMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
var testMatrix = new Matrix33(copyMatrix);
Assert.AreEqual(testMatrix.A11, 1.0);
Assert.AreEqual(testMatrix.A12, -2.0);
Assert.AreEqual(testMatrix.A13, -3.0);
Assert.AreEqual(testMatrix.A21, 4.6);
Assert.AreEqual(testMatrix.A22, 3.2);
Assert.AreEqual(testMatrix.A23, -1.2);
Assert.AreEqual(testMatrix.A31, -0.9);
Assert.AreEqual(testMatrix.A32, 8.1);
Assert.AreEqual(testMatrix.A33, 3.4);
}
public void Add_two_matrices()
{
var matrix = new Matrix33(4, 9, -2,
1, 8, 4,
-3, 2, 5);
var matrixToAdd = new Matrix33(2, 3, 5,
-2, 0, 12,
3, -1, 4);
var actual = matrix + matrixToAdd;
Assert.AreEqual(actual[1, 1], 6, 1E-08);
Assert.AreEqual(actual[1, 2], 12, 1E-08);
Assert.AreEqual(actual[1, 3], 3, 1E-08);
Assert.AreEqual(actual[2, 1], -1, 1E-08);
Assert.AreEqual(actual[2, 2], 8, 1E-08);
Assert.AreEqual(actual[2, 3], 16, 1E-08);
Assert.AreEqual(actual[3, 1], 0, 1E-08);
Assert.AreEqual(actual[3, 2], 1, 1E-08);
Assert.AreEqual(actual[3, 3], 9, 1E-08);
}
public void Multiply_this_matrix_by_another_one()
{
var matrix = new Matrix33(4, 9, -2,
1, 8, 4,
-3, 2, 5);
var otherMatrix = new Matrix33(2, 3, 5,
-2, 0, 12,
3, -1, 4);
var actual = matrix.MultiplyThisByOther(otherMatrix);
Assert.AreEqual(actual[1, 1], -16, 1E-08);
Assert.AreEqual(actual[1, 2], 14, 1E-08);
Assert.AreEqual(actual[1, 3], 120, 1E-08);
Assert.AreEqual(actual[2, 1], -2, 1E-08);
Assert.AreEqual(actual[2, 2], -1, 1E-08);
Assert.AreEqual(actual[2, 3], 117, 1E-08);
Assert.AreEqual(actual[3, 1], 5, 1E-08);
Assert.AreEqual(actual[3, 2], -14, 1E-08);
Assert.AreEqual(actual[3, 3], 29, 1E-08);
}
public void Write_access_matrix_by_index()
{
var testMatrix = new Matrix33();
testMatrix[1, 1] = 1.3;
testMatrix[1, 2] = 3.0;
testMatrix[1, 3] = 5.4;
testMatrix[2, 1] = -9.8;
testMatrix[2, 2] = -42.1;
testMatrix[2, 3] = 4.9;
testMatrix[3, 1] = 7.9;
testMatrix[3, 2] = 1.2;
testMatrix[3, 3] = 4.2;
Assert.AreEqual(testMatrix.A11, 1.3);
Assert.AreEqual(testMatrix.A12, 3.0);
Assert.AreEqual(testMatrix.A13, 5.4);
Assert.AreEqual(testMatrix.A21, -9.8);
Assert.AreEqual(testMatrix.A22, -42.1);
Assert.AreEqual(testMatrix.A23, 4.9);
Assert.AreEqual(testMatrix.A31, 7.9);
Assert.AreEqual(testMatrix.A32, 1.2);
Assert.AreEqual(testMatrix.A33, 4.2);
}
public Matrix33 MultiplyOtherByThis(Matrix33 factor)
{
var a11 = factor.A11 * A11 + factor.A12 * A21 + factor.A13 * A31;
var a12 = factor.A11 * A12 + factor.A12 * A22 + factor.A13 * A32;
var a13 = factor.A11 * A13 + factor.A12 * A23 + factor.A13 * A33;
var a21 = factor.A21 * A11 + factor.A22 * A21 + factor.A23 * A31;
var a22 = factor.A21 * A12 + factor.A22 * A22 + factor.A23 * A32;
var a23 = factor.A21 * A13 + factor.A22 * A23 + factor.A23 * A33;
var a31 = factor.A31 * A11 + factor.A32 * A21 + factor.A33 * A31;
var a32 = factor.A31 * A12 + factor.A32 * A22 + factor.A33 * A32;
var a33 = factor.A31 * A13 + factor.A32 * A23 + factor.A33 * A33;
var retVal = new Matrix33(a11, a12, a13, a21, a22, a23, a31, a32, a33);
return retVal;
}
public void Invalid_write_access_to_matrix_element_4()
{
var testMatrix = new Matrix33();
testMatrix[4, 1] = testMatrix[1, 1];
}
public static Matrix33 operator -(Matrix33 subtrahend1, Matrix33 subtrahend2)
{
var retVal = new Matrix33(subtrahend1.A11 - subtrahend2.A11, subtrahend1.A12 - subtrahend2.A12, subtrahend1.A13 - subtrahend2.A13,
subtrahend1.A21 - subtrahend2.A21, subtrahend1.A22 - subtrahend2.A22, subtrahend1.A23 - subtrahend2.A23,
subtrahend1.A31 - subtrahend2.A31, subtrahend1.A32 - subtrahend2.A32, subtrahend1.A33 - subtrahend2.A33);
return retVal;
}
public Matrix33 CalculateInverse()
{
var det = CalculateDeterminante();
if (Math.Abs(det - 0) < EqualityTolerance)
{
throw new ArgumentException("Determinante of given matrix is 0");
}
var revDet = 1/det;
var a = revDet * (A22 * A33 - A23 * A32);
var b = revDet * (A23 * A31 - A21 * A33);
var c = revDet * (A21 * A32 - A22 * A31);
var d = revDet * (A13 * A32 - A12 * A33);
var e = revDet * (A11 * A33 - A13 * A31);
var f = revDet * (A12 * A31 - A11 * A32);
var g = revDet * (A12 * A23 - A13 * A22);
var h = revDet * (A13 * A21 - A11 * A23);
var i = revDet * (A11 * A22 - A12 * A21);
var result = new Matrix33(a, d, g, b, e, h, c, f, i);
return result;
}
public void Multiply_other_matrix_by_this_one()
{
var matrix = new Matrix33(4, 9, -2,
1, 8, 4,
-3, 2, 5);
var otherMatrix = new Matrix33(2, 3, 5,
-2, 0, 12,
3, -1, 4);
var actual = matrix.MultiplyOtherByThis(otherMatrix);
Assert.AreEqual(actual[1, 1], -4, 1E-08);
Assert.AreEqual(actual[1, 2], 52, 1E-08);
Assert.AreEqual(actual[1, 3], 33, 1E-08);
Assert.AreEqual(actual[2, 1], -44, 1E-08);
Assert.AreEqual(actual[2, 2], 6, 1E-08);
Assert.AreEqual(actual[2, 3], 64, 1E-08);
Assert.AreEqual(actual[3, 1], -1, 1E-08);
Assert.AreEqual(actual[3, 2], 27, 1E-08);
Assert.AreEqual(actual[3, 3], 10, 1E-08);
}
public void Calculate_determinante_of_given_matrix()
{
const double expected = -70.7;
var testMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
var actual = testMatrix.CalculateDeterminante();
Assert.AreEqual(actual, expected, 1E-08);
}
public void Multiply_matrix_by_scalar()
{
const double scalar = 1.5;
var matrix = new Matrix33(4, 9, -2,
1, 8, 4,
-3, 2, 5);
var actual = matrix.MultiplyBy(scalar);
Assert.AreEqual(actual[1, 1], 6, 1E-08);
Assert.AreEqual(actual[1, 2], 13.5, 1E-08);
Assert.AreEqual(actual[1, 3], -3, 1E-08);
Assert.AreEqual(actual[2, 1], 1.5, 1E-08);
Assert.AreEqual(actual[2, 2], 12, 1E-08);
Assert.AreEqual(actual[2, 3], 6, 1E-08);
Assert.AreEqual(actual[3, 1], -4.5, 1E-08);
Assert.AreEqual(actual[3, 2], 3, 1E-08);
Assert.AreEqual(actual[3, 3], 7.5, 1E-08);
}
public void Matrix_is_not_invertible()
{
var testMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
0.0, 0.0, 0.0);
var actual = testMatrix.IsInvertible();
Assert.AreEqual(actual, false);
}
public void Matrix_is_invertible()
{
var testMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
var actual = testMatrix.IsInvertible();
Assert.AreEqual(actual, true);
}
public void Matrix_has_no_inverse()
{
var testMatrix = new Matrix33(1, 2, 3.3,
4, 7,-4,
0, 0, 0);
testMatrix.CalculateInverse();
}
public void Inverse_matrix_should_be_calculated_properly()
{
var testMatrix = new Matrix33(3, 2, 6,
1, 1, 3,
-3, -2, -5);
var actual = testMatrix.CalculateInverse();
Assert.AreEqual(actual.A11, 1, 1E-08);
Assert.AreEqual(actual.A12, -2, 1E-08);
Assert.AreEqual(actual.A13, 0, 1E-08);
Assert.AreEqual(actual.A21, -4, 1E-08);
Assert.AreEqual(actual.A22, 3, 1E-08);
Assert.AreEqual(actual.A23, -3, 1E-08);
Assert.AreEqual(actual.A31, 1, 1E-08);
Assert.AreEqual(actual.A32, 0, 1E-08);
Assert.AreEqual(actual.A33, 1, 1E-08);
}
public static Matrix33 operator +(Matrix33 summand1, Matrix33 summand2)
{
var retVal = new Matrix33(summand1.A11 + summand2.A11, summand1.A12 + summand2.A12, summand1.A13 + summand2.A13,
summand1.A21 + summand2.A21, summand1.A22 + summand2.A22, summand1.A23 + summand2.A23,
summand1.A31 + summand2.A31, summand1.A32 + summand2.A32, summand1.A33 + summand2.A33);
return retVal;
}
public void Multiply_vector_by_matrix()
{
var vector = new Vector3(3.0, 2.0, 4.0);
var matrix = new Matrix33(4, 9, -2,
1, 8, 4,
-3, 2, 5);
var actual = matrix.MultiplyVectorByMatrix(vector);
Assert.AreEqual(actual.X, 2, 1E-08);
Assert.AreEqual(actual.Y, 51, 1E-08);
Assert.AreEqual(actual.Z, 22, 1E-08);
}
public static Matrix33 Transpose(Matrix33 orig)
{
var transposedMatrix = new Matrix33(orig);
transposedMatrix.Transpose();
return transposedMatrix;
}
public void Read_access_matrix_by_index()
{
var testMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
var a11 = testMatrix[1, 1];
var a12 = testMatrix[1, 2];
var a13 = testMatrix[1, 3];
var a21 = testMatrix[2, 1];
var a22 = testMatrix[2, 2];
var a23 = testMatrix[2, 3];
var a31 = testMatrix[3, 1];
var a32 = testMatrix[3, 2];
var a33 = testMatrix[3, 3];
Assert.AreEqual(a11, 1.0);
Assert.AreEqual(a12, -2.0);
Assert.AreEqual(a13, -3.0);
Assert.AreEqual(a21, 4.6);
Assert.AreEqual(a22, 3.2);
Assert.AreEqual(a23, -1.2);
Assert.AreEqual(a31, -0.9);
Assert.AreEqual(a32, 8.1);
Assert.AreEqual(a33, 3.4);
}
public Matrix33 MultiplyBy(double scalar)
{
var retVal = new Matrix33(A11*scalar, A12*scalar, A13*scalar,
A21*scalar, A22*scalar, A23*scalar,
A31*scalar, A32*scalar, A33*scalar);
return retVal;
}
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);
}
public Matrix33 MultiplyThisByOther(Matrix33 factor)
{
var a11 = A11*factor.A11 + A12*factor.A21 + A13*factor.A31;
var a12 = A11*factor.A12 + A12*factor.A22 + A13*factor.A32;
var a13 = A11*factor.A13 + A12*factor.A23 + A13*factor.A33;
var a21 = A21*factor.A11 + A22*factor.A21 + A23*factor.A31;
var a22 = A21*factor.A12 + A22*factor.A22 + A23*factor.A32;
var a23 = A21*factor.A13 + A22*factor.A23 + A23*factor.A33;
var a31 = A31*factor.A11 + A32*factor.A21 + A33*factor.A31;
var a32 = A31*factor.A12 + A32*factor.A22 + A33*factor.A32;
var a33 = A31*factor.A13 + A32*factor.A23 + A33*factor.A33;
var retVal = new Matrix33(a11, a12, a13, a21, a22, a23, a31, a32, a33);
return retVal;
}
public void The_given_matrix_should_be_transposed()
{
var origMatrix = new Matrix33(1.0, -2.0, -3.0,
4.6, 3.2, -1.2,
-0.9, 8.1, 3.4);
var actual = Matrix33.Transpose(origMatrix);
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);
}
// 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 Invalid_read_access_to_matrix_element_3()
{
var testMatrix = new Matrix33();
testMatrix[1, 1] = testMatrix[3, 0];
}