| public SetElement ( int rowIndex, int columnIndex, double element ) : void | ||
| rowIndex | int | The number of the row where the element will be added |
| columnIndex | int | The number of the column where the element will be added |
| element | double | |
| return | void | |
public void Matrix_CanBeMultipliedTest()
{
Matrix matrix1 = new Matrix(3, 2);
matrix1.SetElement(0, 0, 1);
matrix1.SetElement(0, 1, 1);
matrix1.SetElement(1, 0, 2);
matrix1.SetElement(1, 1, 0);
matrix1.SetElement(2, 0, 0);
matrix1.SetElement(2, 1, 3);
Matrix matrix2 = new Matrix(2, 4);
matrix2.SetElement(0, 0, 0);
matrix2.SetElement(0, 1, 1);
matrix2.SetElement(0, 2, 1);
matrix2.SetElement(0, 3, 0);
matrix2.SetElement(1, 0, 2);
matrix2.SetElement(1, 1, 3);
matrix2.SetElement(1, 2, 0);
matrix2.SetElement(1, 3, 1);
bool result = matrix1.CanBeMultipliedBy(matrix2);
result.Should().BeTrue();
}
public void Matrix_StandardMatrixMultiplyTest()
{
Matrix matrix1 = new Matrix(3, 2);
matrix1.SetElement(0, 0, 1);
matrix1.SetElement(0, 1, 1);
matrix1.SetElement(1, 0, 2);
matrix1.SetElement(1, 1, 0);
matrix1.SetElement(2, 0, 0);
matrix1.SetElement(2, 1, 3);
Matrix matrix2 = new Matrix(2, 4);
matrix2.SetElement(0, 0, 0);
matrix2.SetElement(0, 1, 1);
matrix2.SetElement(0, 2, 1);
matrix2.SetElement(0, 3, 0);
matrix2.SetElement(1, 0, 2);
matrix2.SetElement(1, 1, 3);
matrix2.SetElement(1, 2, 0);
matrix2.SetElement(1, 3, 1);
Matrix answerMatrix = new Matrix(3, 4);
answerMatrix.SetElement(0, 0, 2);
answerMatrix.SetElement(0, 1, 4);
answerMatrix.SetElement(0, 2, 1);
answerMatrix.SetElement(0, 3, 1);
answerMatrix.SetElement(1, 0, 0);
answerMatrix.SetElement(1, 1, 2);
answerMatrix.SetElement(1, 2, 2);
answerMatrix.SetElement(1, 3, 0);
answerMatrix.SetElement(2, 0, 6);
answerMatrix.SetElement(2, 1, 9);
answerMatrix.SetElement(2, 2, 0);
answerMatrix.SetElement(2, 3, 3);
Matrix resultingMatrix = matrix1.MultiplyBy(matrix2);
bool answer = (resultingMatrix == answerMatrix);
answer.Should().BeTrue();
}
public void Matrix_IdentityMatrixAdditionTest()
{
Matrix matrix1 = new Matrix(3, 2);
matrix1.SetElement(0, 0, 1);
matrix1.SetElement(0, 1, 1);
matrix1.SetElement(1, 0, 2);
matrix1.SetElement(1, 1, 0);
matrix1.SetElement(2, 0, 0);
matrix1.SetElement(2, 1, 3);
Matrix matrix2 = new Matrix(3, 2);
matrix2.SetElement(0, 0, 0);
matrix2.SetElement(0, 1, 0);
matrix2.SetElement(1, 0, 0);
matrix2.SetElement(1, 1, 0);
matrix2.SetElement(2, 0, 0);
matrix2.SetElement(2, 1, 0);
Matrix resultingMatrix = matrix1.AddTo(matrix2);
bool answer = (resultingMatrix == matrix1);
answer.Should().BeTrue();
}
public void Matrix_CannotBeAddedTest()
{
Matrix matrix1 = new Matrix(3, 2);
matrix1.SetElement(0, 0, 1);
matrix1.SetElement(0, 1, 1);
matrix1.SetElement(1, 0, 2);
matrix1.SetElement(1, 1, 0);
matrix1.SetElement(2, 0, 0);
matrix1.SetElement(2, 1, 3);
Matrix matrix2 = new Matrix(2, 3);
matrix2.SetElement(0, 0, 0);
matrix2.SetElement(0, 1, 1);
matrix2.SetElement(0, 2, 1);
matrix2.SetElement(1, 0, 0);
matrix2.SetElement(1, 1, 2);
matrix2.SetElement(1, 2, 3);
bool result = matrix1.CanBeAddedTo(matrix2);
result.Should().BeFalse();
}
public void Matrix_RemoveRowsAndColumnsTest()
{
Matrix testMatrix = new Matrix(3,3);
testMatrix.InsertMatrixAt(Matrix.IdentityMatrix(2), 1, 1);
int[] rowsToRemove = { 0 };
int[] columnsToRemove = { 0, 1 };
Matrix actualMatrix = testMatrix.RemoveRows(rowsToRemove);
actualMatrix = actualMatrix.RemoveColumns(columnsToRemove);
Matrix expectedMatrix = new Matrix(2, 1);
expectedMatrix.SetElement(1, 0, 1.0);
(actualMatrix == expectedMatrix).Should().BeTrue();
}
/// <summary>
/// Returns a matrix that can be multiplied by another matrix to represent a rotation of that matrix about the passed axis line by the specified angle
/// </summary>>
public static Matrix RotationMatrixAboutOrigin(Rotation passedRotation)
{
Matrix rotationMatrix = new Matrix(3);
Direction rotationUnitVector = passedRotation.AxisOfRotation.Direction;
double unitX = rotationUnitVector.X; //Projection onto x-axis
double unitY = rotationUnitVector.Y;
double unitZ = rotationUnitVector.Z;
double theta = passedRotation.RotationAngle.InRadians.Value;
double sinTheta = Math.Sin(theta);
double cosTheta = Math.Cos(theta);
double row0column0 = Math.Cos(theta) + unitX*unitX*(1 - Math.Cos(theta));
double row0column1 = unitX*unitY*(1 - Math.Cos(theta)) - unitZ*Math.Sin(theta);
double row0column2 = unitX*unitZ*(1 - Math.Cos(theta)) + unitY*Math.Sin(theta);
double row1column0 = unitY*unitX*(1 - Math.Cos(theta)) + unitZ*Math.Sin(theta);
double row1column1 = Math.Cos(theta) + unitY*unitY*(1 - Math.Cos(theta));
double row1column2 = unitY*unitZ*(1 - Math.Cos(theta)) - unitX*Math.Sin(theta);
double row2column0 = unitZ*unitX*(1 - Math.Cos(theta)) - unitY*Math.Sin(theta);
double row2column1 = unitZ*unitY*(1 - Math.Cos(theta)) + unitX*Math.Sin(theta);
double row2column2 = Math.Cos(theta) + unitZ*unitZ*(1 - Math.Cos(theta));
rotationMatrix.SetElement(0, 0, row0column0);
rotationMatrix.SetElement(0, 1, row0column1);
rotationMatrix.SetElement(0, 2, row0column2);
rotationMatrix.SetElement(1, 0, row1column0);
rotationMatrix.SetElement(1, 1, row1column1);
rotationMatrix.SetElement(1, 2, row1column2);
rotationMatrix.SetElement(2, 0, row2column0);
rotationMatrix.SetElement(2, 1, row2column1);
rotationMatrix.SetElement(2, 2, row2column2);
return rotationMatrix;
}
// --------------------------------------------------------------------------------------------------------------
/// <summary>
/// Performs Doolittle decomposition with partial pivoting and returns a combined LU matrix
/// </summary>
/// <param name="permutationArray">Keeps track of how the matrices have been rearranged during the calculation</param>
/// <param name="toggle">is +1 or -1 (even or odd)</param>
/// <returns></returns>
public Matrix Decompose(out int[] permutationArray, out int toggle)
{
if (!this.IsSquare())
{
throw new Exception("LU-Decomposition can only be found for a square matrix");
}
Matrix decomposedMatrix = new Matrix(_matrix); // copy this matrix before messing with it
permutationArray = new int[NumberOfRows]; // set up row permutation result
for (int i = 0; i < NumberOfRows; ++i)
{
permutationArray[i] = i;
}
toggle = 1; // toggle tracks row swaps. +1 -> even, -1 -> odd
//Loop through the columns
for (int columnIndex = 0; columnIndex < NumberOfRows - 1; columnIndex++)
{
//Find largest value in the current column
double maxOfColumn = Math.Abs(decomposedMatrix.GetElement(columnIndex, columnIndex));
int pivotRowIndex = columnIndex;
// loop through all of the rows in this column
for (int rowIndex = columnIndex + 1; rowIndex < NumberOfRows; rowIndex++)
{
if (Math.Abs(decomposedMatrix.GetElement(rowIndex, columnIndex)) > maxOfColumn)
{
maxOfColumn = Math.Abs(decomposedMatrix.GetElement(rowIndex, columnIndex));
pivotRowIndex = rowIndex;
}
}
if (pivotRowIndex != columnIndex) // if largest value not on pivot, swap rows
{
double[] rowPtr = decomposedMatrix.GetRow(pivotRowIndex);
decomposedMatrix.SetRow(pivotRowIndex, decomposedMatrix.GetRow(columnIndex));
decomposedMatrix.SetRow(columnIndex, rowPtr);
int tmp = permutationArray[pivotRowIndex]; // and swap permutation info
permutationArray[pivotRowIndex] = permutationArray[columnIndex];
permutationArray[columnIndex] = tmp;
toggle = -toggle; // adjust the row-swap toggle
}
//Check to see if there is a zero on the diagonal. If so, try to get rid of it by swapping with a row that is beneath the row with a zero on the diagonal
// double elementOnDiagonal = decomposedMatrix.GetElement(columnIndex, columnIndex);
//if (Math.Round(elementOnDiagonal, 6) == 0.0)
//{
// // find a good row to swap
// int goodRow = -1;
// for (int row = columnIndex + 1; row < decomposedMatrix.NumberOfRows; row++)
// {
// double element = decomposedMatrix.GetElement(row, columnIndex);
// if (Math.Round(element, 6) != 0.0)
// {
// goodRow = row;
// }
// }
// if (goodRow != -1)
// {
// // swap rows so 0.0 no longer on diagonal
// double[] rowPtr = decomposedMatrix.GetRow(goodRow);
// decomposedMatrix.SetRow(goodRow, decomposedMatrix.GetRow(columnIndex));
// decomposedMatrix.SetRow(columnIndex, rowPtr);
// int tmp = permutationArray[goodRow]; // and swap perm info
// permutationArray[goodRow] = permutationArray[columnIndex];
// permutationArray[columnIndex] = tmp;
// toggle = -toggle; // adjust the row-swap toggle
// }
// else
// {
// // throw new Exception("Cannot use Doolittle's method on this matrix");
// }
//}
//Find the next value to insert into the decomposed matrix
for (int rowIndex = columnIndex + 1; rowIndex < this.NumberOfRows; ++rowIndex)
{
double valueToStore = decomposedMatrix.GetElement(rowIndex, columnIndex)/decomposedMatrix.GetElement(columnIndex, columnIndex);
decomposedMatrix.SetElement(rowIndex, columnIndex, valueToStore);
for (int nextColumnIndex = columnIndex + 1; nextColumnIndex < NumberOfRows; ++nextColumnIndex)
{
double valueToStore2 = decomposedMatrix.GetElement(rowIndex, nextColumnIndex) - decomposedMatrix.GetElement(rowIndex, columnIndex)*decomposedMatrix.GetElement(columnIndex, nextColumnIndex);
decomposedMatrix.SetElement(rowIndex, nextColumnIndex, valueToStore2);
}
}
} // main column loop
return decomposedMatrix;
} // MatrixDecompose
/// <summary>
/// Returns an Identity Matrix of the specified size. That is, a square matrix with 1's on the diagonal and 0's everywhere else.
/// </summary>
public static Matrix IdentityMatrix(int passedNumberOfRowsAndColumns)
{
// returns an n x n Identity matrix
Matrix result = new Matrix(passedNumberOfRowsAndColumns);
for (int i = 0; i < passedNumberOfRowsAndColumns; i++)
{
result.SetElement(i, i, 1.0);
}
return result;
}
/// <summary>
/// Returns a matrix that is this matrix with the absolute value function applied to each element
/// </summary>
public Matrix AbsoluteValue()
{
Matrix resultingMatrix = new Matrix(NumberOfRows, NumberOfColumns);
for (int i = 0; i < NumberOfRows; i++)
{
for (int j = 0; j < NumberOfColumns; j++)
{
double newElement = Math.Abs(GetElement(i, j));
resultingMatrix.SetElement(i, j, newElement);
}
}
return resultingMatrix;
}
/// <summary>
/// Converts the given rotation Matrix into a Quaternion, which also represents orientation but in a more mathematically unique way
/// since only two quaternions represent the same orientation, and they have the following relation to each other: q = -q
/// </summary>
/// <returns>Returns the Quarternion representation of this Rotation Matrix, stored in a 4x1 Matrix</returns>
public Matrix ConvertRotationMatrixToQuaternion()
{
//Follows this question asked by user1283674 on stack overflow
//http://stackoverflow.com/questions/21455139/matrix-rotation-to-quaternion
// Output quaternion
double w, x, y, z;
// Determine which of w,x,y, or z has the largest absolute value
double fourWSquaredMinus1 = this.GetElement(0, 0) + this.GetElement(1, 1) + this.GetElement(2, 2);
double fourXSquaredMinus1 = this.GetElement(0, 0) - this.GetElement(1, 1) - this.GetElement(2, 2);
double fourYSquaredMinus1 = this.GetElement(1, 1) - this.GetElement(0, 0) - this.GetElement(2, 2);
double fourZSquaredMinus1 = this.GetElement(2, 2) - this.GetElement(0, 0) - this.GetElement(1, 1);
int biggestIndex = 0;
double fourBiggestSquaredMinus1 = fourWSquaredMinus1;
if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
{
fourBiggestSquaredMinus1 = fourXSquaredMinus1;
biggestIndex = 1;
}
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
{
fourBiggestSquaredMinus1 = fourYSquaredMinus1;
biggestIndex = 2;
}
if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
{
fourBiggestSquaredMinus1 = fourZSquaredMinus1;
biggestIndex = 3;
}
// Perform square root and division
//(I have also seen it this way: mult = 0.25 * Math.Sqrt (fourBiggestSquaredMinus1 + 1.0 ) * 2
// and then you divide by mult below instead of * i.e x = (this.GetElement(1, 2) - this.GetElement(2, 1)) / mult;
// and both ways seem to work the same)
double biggestVal = Math.Sqrt(fourBiggestSquaredMinus1 + 1.0)*0.5;
double mult = 0.25/biggestVal;
// Apply table to compute quaternion values
switch (biggestIndex)
{
case 0:
w = biggestVal;
x = (this.GetElement(1, 2) - this.GetElement(2, 1))*mult;
y = (this.GetElement(2, 0) - this.GetElement(0, 2))*mult;
z = (this.GetElement(0, 1) - this.GetElement(1, 0))*mult;
break;
case 1:
x = biggestVal;
w = (this.GetElement(1, 2) - this.GetElement(2, 1))*mult;
y = (this.GetElement(0, 1) + this.GetElement(1, 0))*mult;
z = (this.GetElement(2, 0) + this.GetElement(0, 2))*mult;
break;
case 2:
y = biggestVal;
w = (this.GetElement(2, 0) - this.GetElement(0, 2))*mult;
x = (this.GetElement(0, 1) + this.GetElement(1, 0))*mult;
z = (this.GetElement(1, 2) + this.GetElement(2, 1))*mult;
break;
case 3:
z = biggestVal;
w = (this.GetElement(0, 1) - this.GetElement(1, 0))*mult;
x = (this.GetElement(2, 0) + this.GetElement(0, 2))*mult;
y = (this.GetElement(1, 2) + this.GetElement(2, 1))*mult;
break;
default:
throw new Exception("Error creating quaternion");
}
var returnMatrix = new Matrix(4, 1);
returnMatrix.SetElement(0, 0, w);
returnMatrix.SetElement(1, 0, x);
returnMatrix.SetElement(2, 0, y);
returnMatrix.SetElement(3, 0, z);
return returnMatrix;
//return new double[] { w, x, y, z };
}
public static Matrix ProjectiveMatrixToRotationMatrix(Matrix matrix)
{
var newMatrix = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
newMatrix.SetElement(i, j, matrix.GetElement(i, j));
}
}
return newMatrix;
}
// --------------------------------------------------------------------------------------------------------------
// --------------------------------------------------------------------------------------------------------------
/// <summary>
/// Returns Matrix with only the values in the lower triangle (below main diagonal) intact. Puts 1.0's on diagonal and 0.0's in upper triangle
/// </summary>
/// <returns></returns>
public Matrix ExtractLower()
{
Matrix LowerPart = new Matrix(NumberOfRows, NumberOfColumns);
for (int i = 0; i < NumberOfRows; ++i)
{
for (int j = 0; j < NumberOfColumns; ++j)
{
if (i == j)
{
//This element is on the main diagonal
LowerPart.SetElement(i, j, 1.0);
}
else if (i > j)
{
//This element is below the main diagonal
LowerPart.SetElement(i, j, this.GetElement(i, j));
}
}
}
return LowerPart;
}
/// <summary>
/// Creates the cofactor matrix corresponding to this matrix
/// </summary>
public Matrix GenerateCofactorMatrix()
{
Matrix cofactorMatrix = new Matrix(NumberOfRows, NumberOfColumns);
for (int rowIndex = 0; rowIndex < NumberOfRows; rowIndex++)
{
for (int columnIndex = 0; columnIndex < NumberOfColumns; columnIndex++)
{
double cofactor = GetCofactor(rowIndex, columnIndex);
cofactorMatrix.SetElement(rowIndex, columnIndex, cofactor);
}
}
return cofactorMatrix;
}
private Matrix _matrixOfRotationAboutOrigin()
{
Matrix rotationMatrix = new Matrix(4, 4);
Direction rotationUnitVector = this.AxisOfRotation.Direction;
double unitX = rotationUnitVector.X; //Projection onto x-axis
double unitY = rotationUnitVector.Y;
double unitZ = rotationUnitVector.Z;
Angle theta = this.RotationAngle;
double sinTheta = Angle.Sine(theta);
double cosTheta = Angle.Cosine(theta);
double row0column0 = cosTheta + unitX * unitX * (1 - cosTheta);
double row0column1 = unitX * unitY * (1 - cosTheta) - unitZ * sinTheta;
double row0column2 = unitX * unitZ * (1 - cosTheta) + unitY * sinTheta;
double row1column0 = unitY * unitX * (1 - cosTheta) + unitZ * sinTheta;
double row1column1 = cosTheta + unitY * unitY * (1 - cosTheta);
double row1column2 = unitY * unitZ * (1 - cosTheta) - unitX * sinTheta;
double row2column0 = unitZ * unitX * (1 - cosTheta) - unitY * sinTheta;
double row2column1 = unitZ * unitY * (1 - cosTheta) + unitX * sinTheta;
double row2column2 = cosTheta + unitZ * unitZ * (1 - cosTheta);
rotationMatrix.SetElement(0, 0, row0column0);
rotationMatrix.SetElement(0, 1, row0column1);
rotationMatrix.SetElement(0, 2, row0column2);
rotationMatrix.SetElement(1, 0, row1column0);
rotationMatrix.SetElement(1, 1, row1column1);
rotationMatrix.SetElement(1, 2, row1column2);
rotationMatrix.SetElement(2, 0, row2column0);
rotationMatrix.SetElement(2, 1, row2column1);
rotationMatrix.SetElement(2, 2, row2column2);
rotationMatrix.SetElement(3, 3, 1.0);
return rotationMatrix;
}