| public GetElement ( int rowIndex, int columnIndex ) : double | ||
| rowIndex | int | The row number of the desired element |
| columnIndex | int | The column number of the desired element |
| return | double | |
/// <summary>
/// Inserts the passed Matrix into this Matrix starting at the specified index
/// </summary>
public Matrix InsertMatrixAt(Matrix passedMatrix, int destinationRow, int destinationColumn)
{
for (int row = 0; row < passedMatrix.NumberOfRows; row++)
{
for (int col = 0; col < passedMatrix.NumberOfColumns; col++)
{
this.SetElement(row + destinationRow, col + destinationColumn, passedMatrix.GetElement(row, col));
}
}
return this;
}
// --------------------------------------------------------------------------------------------------------------
/// <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
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;
}