/**
* Converts {@link FMatrix3x3} into {@link FMatrixRMaj}.
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static FMatrixRMaj convert(FMatrix3x3 input, FMatrixRMaj output)
{
if (output == null)
{
output = new FMatrixRMaj(3, 3);
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.data[0] = input.a11;
output.data[1] = input.a12;
output.data[2] = input.a13;
output.data[3] = input.a21;
output.data[4] = input.a22;
output.data[5] = input.a23;
output.data[6] = input.a31;
output.data[7] = input.a32;
output.data[8] = input.a33;
return(output);
}
/**
* Converts {@link FMatrixRMaj} into {@link FMatrix3x3}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static FMatrix3x3 convert(FMatrixRMaj input, FMatrix3x3 output)
{
if (output == null)
{
output = new FMatrix3x3();
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.a11 = input.data[0];
output.a12 = input.data[1];
output.a13 = input.data[2];
output.a21 = input.data[3];
output.a22 = input.data[4];
output.a23 = input.data[5];
output.a31 = input.data[6];
output.a32 = input.data[7];
output.a33 = input.data[8];
return(output);
}
/**
* Extracts the column from the matrix a.
* @param a Input matrix
* @param column Which column is to be extracted
* @param output output. Storage for the extracted column. If null then a new vector will be returned.
* @return The extracted column.
*/
public static FMatrix3 extractColumn(FMatrix3x3 a, int column, FMatrix3 output)
{
if (output == null)
{
output = new FMatrix3();
}
switch (column)
{
case 0:
output.a1 = a.a11;
output.a2 = a.a21;
output.a3 = a.a31;
break;
case 1:
output.a1 = a.a12;
output.a2 = a.a22;
output.a3 = a.a32;
break;
case 2:
output.a1 = a.a13;
output.a2 = a.a23;
output.a3 = a.a33;
break;
default:
throw new ArgumentException("Out of bounds column. column = " + column);
}
return(output);
}
/**
* Extracts the row from the matrix a.
* @param a Input matrix
* @param row Which row is to be extracted
* @param output output. Storage for the extracted row. If null then a new vector will be returned.
* @return The extracted row.
*/
public static FMatrix3 extractRow(FMatrix3x3 a, int row, FMatrix3 output)
{
if (output == null)
{
output = new FMatrix3();
}
switch (row)
{
case 0:
output.a1 = a.a11;
output.a2 = a.a12;
output.a3 = a.a13;
break;
case 1:
output.a1 = a.a21;
output.a2 = a.a22;
output.a3 = a.a23;
break;
case 2:
output.a1 = a.a31;
output.a2 = a.a32;
output.a3 = a.a33;
break;
default:
throw new ArgumentException("Out of bounds row. row = " + row);
}
return(output);
}
/**
* Computes the determinant using minor matrices.<br>
* WARNING: Potentially less stable than using LU decomposition.
*
* @param mat Input matrix. Not modified.
* @return The determinant.
*/
public static float det(FMatrix3x3 mat)
{
float a = mat.a11 * (mat.a22 * mat.a33 - mat.a23 * mat.a32);
float b = mat.a12 * (mat.a21 * mat.a33 - mat.a23 * mat.a31);
float c = mat.a13 * (mat.a21 * mat.a32 - mat.a31 * mat.a22);
return(a - b + c);
}
public static float fastNormF(FMatrix3x3 M)
{
float sum = 0;
sum += M.a11 * M.a11 + M.a12 * M.a12 + M.a13 * M.a13;
sum += M.a21 * M.a21 + M.a22 * M.a22 + M.a23 * M.a23;
sum += M.a31 * M.a31 + M.a32 * M.a32 + M.a33 * M.a33;
return((float)Math.Sqrt(sum));
}
/**
* <p>Performs an element by element division operation:<br>
* <br>
* c<sub>ij</sub> = a<sub>ij</sub> / b<sub>ij</sub> <br>
* </p>
* @param a The left matrix in the division operation. Not modified.
* @param b The right matrix in the division operation. Not modified.
* @param c Where the results of the operation are stored. Modified.
*/
public static void elementDiv(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 = a.a11 / b.a11;
c.a12 = a.a12 / b.a12;
c.a13 = a.a13 / b.a13;
c.a21 = a.a21 / b.a21;
c.a22 = a.a22 / b.a22;
c.a23 = a.a23 / b.a23;
c.a31 = a.a31 / b.a31;
c.a32 = a.a32 / b.a32;
c.a33 = a.a33 / b.a33;
}
/**
* <p>Performs the following operation:<br>
* <br>
* c = a + b <br>
* c<sub>ij</sub> = a<sub>ij</sub> + b<sub>ij</sub> <br>
* </p>
*
* <p>
* Matrix C can be the same instance as Matrix A and/or B.
* </p>
*
* @param a A Matrix. Not modified.
* @param b A Matrix. Not modified.
* @param c A Matrix where the results are stored. Modified.
*/
public static void add(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 = a.a11 + b.a11;
c.a12 = a.a12 + b.a12;
c.a13 = a.a13 + b.a13;
c.a21 = a.a21 + b.a21;
c.a22 = a.a22 + b.a22;
c.a23 = a.a23 + b.a23;
c.a31 = a.a31 + b.a31;
c.a32 = a.a32 + b.a32;
c.a33 = a.a33 + b.a33;
}
/**
* <p>
* Performs the following operation:<br>
* <br>
* c += a * b<sup>T</sup> <br>
* c<sub>ij</sub> += ∑<sub>k=1:n</sub> { a<sub>ik</sub> * b<sub>jk</sub>}
* </p>
*
* @param a The left matrix in the multiplication operation. Not modified.
* @param b The right matrix in the multiplication operation. Not modified.
* @param c Where the results of the operation are stored. Modified.
*/
public static void multAddTransB(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 += a.a11 * b.a11 + a.a12 * b.a12 + a.a13 * b.a13;
c.a12 += a.a11 * b.a21 + a.a12 * b.a22 + a.a13 * b.a23;
c.a13 += a.a11 * b.a31 + a.a12 * b.a32 + a.a13 * b.a33;
c.a21 += a.a21 * b.a11 + a.a22 * b.a12 + a.a23 * b.a13;
c.a22 += a.a21 * b.a21 + a.a22 * b.a22 + a.a23 * b.a23;
c.a23 += a.a21 * b.a31 + a.a22 * b.a32 + a.a23 * b.a33;
c.a31 += a.a31 * b.a11 + a.a32 * b.a12 + a.a33 * b.a13;
c.a32 += a.a31 * b.a21 + a.a32 * b.a22 + a.a33 * b.a23;
c.a33 += a.a31 * b.a31 + a.a32 * b.a32 + a.a33 * b.a33;
}
/**
* Sets all the diagonal elements equal to one and everything else equal to zero.
* If this is a square matrix then it will be an identity matrix.
*
* @param a A matrix.
*/
public static void setIdentity(FMatrix3x3 a)
{
a.a11 = 1;
a.a21 = 0;
a.a31 = 0;
a.a12 = 0;
a.a22 = 1;
a.a32 = 0;
a.a13 = 0;
a.a23 = 0;
a.a33 = 1;
}
/**
* <p>Performs an element by element division operation:<br>
* <br>
* a<sub>ij</sub> = a<sub>ij</sub> / b<sub>ij</sub> <br>
* </p>
* @param a The left matrix in the division operation. Modified.
* @param b The right matrix in the division operation. Not modified.
*/
public static void elementDiv(FMatrix3x3 a, FMatrix3x3 b)
{
a.a11 /= b.a11;
a.a12 /= b.a12;
a.a13 /= b.a13;
a.a21 /= b.a21;
a.a22 /= b.a22;
a.a23 /= b.a23;
a.a31 /= b.a31;
a.a32 /= b.a32;
a.a33 /= b.a33;
}
/**
* <p>
* Performs the following operation:<br>
* <br>
* c = a * b<sup>T</sup> <br>
* c<sub>ij</sub> = ∑<sub>k=1:n</sub> { a<sub>ik</sub> * b<sub>jk</sub>}
* </p>
*
* @param a The left matrix in the multiplication operation. Not modified.
* @param b The right matrix in the multiplication operation. Not modified.
* @param c Where the results of the operation are stored. Modified.
*/
public static void multTransB(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 = a.a11 * b.a11 + a.a12 * b.a12 + a.a13 * b.a13;
c.a12 = a.a11 * b.a21 + a.a12 * b.a22 + a.a13 * b.a23;
c.a13 = a.a11 * b.a31 + a.a12 * b.a32 + a.a13 * b.a33;
c.a21 = a.a21 * b.a11 + a.a22 * b.a12 + a.a23 * b.a13;
c.a22 = a.a21 * b.a21 + a.a22 * b.a22 + a.a23 * b.a23;
c.a23 = a.a21 * b.a31 + a.a22 * b.a32 + a.a23 * b.a33;
c.a31 = a.a31 * b.a11 + a.a32 * b.a12 + a.a33 * b.a13;
c.a32 = a.a31 * b.a21 + a.a32 * b.a22 + a.a33 * b.a23;
c.a33 = a.a31 * b.a31 + a.a32 * b.a32 + a.a33 * b.a33;
}
/**
* <p>
* Performs an element by element scalar multiplication.<br>
* <br>
* b<sub>ij</sub> = α*a<sub>ij</sub>
* </p>
*
* @param alpha the amount each element is multiplied by.
* @param a The matrix that is to be scaled. Not modified.
* @param b Where the scaled matrix is stored. Modified.
*/
public static void scale(float alpha, FMatrix3x3 a, FMatrix3x3 b)
{
b.a11 = a.a11 * alpha;
b.a12 = a.a12 * alpha;
b.a13 = a.a13 * alpha;
b.a21 = a.a21 * alpha;
b.a22 = a.a22 * alpha;
b.a23 = a.a23 * alpha;
b.a31 = a.a31 * alpha;
b.a32 = a.a32 * alpha;
b.a33 = a.a33 * alpha;
}
/**
* <p>
* Performs an in-place element by element scalar multiplication.<br>
* <br>
* a<sub>ij</sub> = α*a<sub>ij</sub>
* </p>
*
* @param a The matrix that is to be scaled. Modified.
* @param alpha the amount each element is multiplied by.
*/
public static void scale(float alpha, FMatrix3x3 a)
{
a.a11 *= alpha;
a.a12 *= alpha;
a.a13 *= alpha;
a.a21 *= alpha;
a.a22 *= alpha;
a.a23 *= alpha;
a.a31 *= alpha;
a.a32 *= alpha;
a.a33 *= alpha;
}
/**
* <p>
* Performs an in-place element by element scalar division. Scalar denominator.<br>
* <br>
* a<sub>ij</sub> = a<sub>ij</sub>/α
* </p>
*
* @param a The matrix whose elements are to be divided. Modified.
* @param alpha the amount each element is divided by.
*/
public static void divide(FMatrix3x3 a, float alpha)
{
a.a11 /= alpha;
a.a12 /= alpha;
a.a13 /= alpha;
a.a21 /= alpha;
a.a22 /= alpha;
a.a23 /= alpha;
a.a31 /= alpha;
a.a32 /= alpha;
a.a33 /= alpha;
}
/**
* <p>Performs the following operation:<br>
* <br>
* a = a - b <br>
* a<sub>ij</sub> = a<sub>ij</sub> - b<sub>ij</sub> <br>
* </p>
*
* @param a A Matrix. Modified.
* @param b A Matrix. Not modified.
*/
public static void subtractEquals(FMatrix3x3 a, FMatrix3x3 b)
{
a.a11 -= b.a11;
a.a12 -= b.a12;
a.a13 -= b.a13;
a.a21 -= b.a21;
a.a22 -= b.a22;
a.a23 -= b.a23;
a.a31 -= b.a31;
a.a32 -= b.a32;
a.a33 -= b.a33;
}
/**
* <p>Performs the following operation:<br>
* <br>
* a = a + b <br>
* a<sub>ij</sub> = a<sub>ij</sub> + b<sub>ij</sub> <br>
* </p>
*
* @param a A Matrix. Modified.
* @param b A Matrix. Not modified.
*/
public static void addEquals(FMatrix3x3 a, FMatrix3x3 b)
{
a.a11 += b.a11;
a.a12 += b.a12;
a.a13 += b.a13;
a.a21 += b.a21;
a.a22 += b.a22;
a.a23 += b.a23;
a.a31 += b.a31;
a.a32 += b.a32;
a.a33 += b.a33;
}
/**
* <p>
* Sets every element in the matrix to the specified value.<br>
* <br>
* a<sub>ij</sub> = value
* <p>
*
* @param a A matrix whose elements are about to be set. Modified.
* @param v The value each element will have.
*/
public static void fill(FMatrix3x3 a, float v)
{
a.a11 = v;
a.a12 = v;
a.a13 = v;
a.a21 = v;
a.a22 = v;
a.a23 = v;
a.a31 = v;
a.a32 = v;
a.a33 = v;
}
/**
* <p>
* Changes the sign of every element in the matrix.<br>
* <br>
* a<sub>ij</sub> = -a<sub>ij</sub>
* </p>
*
* @param a A matrix. Modified.
*/
public static void changeSign(FMatrix3x3 a)
{
a.a11 = -a.a11;
a.a12 = -a.a12;
a.a13 = -a.a13;
a.a21 = -a.a21;
a.a22 = -a.a22;
a.a23 = -a.a23;
a.a31 = -a.a31;
a.a32 = -a.a32;
a.a33 = -a.a33;
}
/**
* <p>
* Performs an element by element scalar division. Scalar denominator.<br>
* <br>
* b<sub>ij</sub> = a<sub>ij</sub> /α
* </p>
*
* @param alpha the amount each element is divided by.
* @param a The matrix whose elements are to be divided. Not modified.
* @param b Where the results are stored. Modified.
*/
public static void divide(FMatrix3x3 a, float alpha, FMatrix3x3 b)
{
b.a11 = a.a11 / alpha;
b.a12 = a.a12 / alpha;
b.a13 = a.a13 / alpha;
b.a21 = a.a21 / alpha;
b.a22 = a.a22 / alpha;
b.a23 = a.a23 / alpha;
b.a31 = a.a31 / alpha;
b.a32 = a.a32 / alpha;
b.a33 = a.a33 / alpha;
}
/**
* <p>Performs the following operation:<br>
* <br>
* c = a - b <br>
* c<sub>ij</sub> = a<sub>ij</sub> - b<sub>ij</sub> <br>
* </p>
*
* <p>
* Matrix C can be the same instance as Matrix A and/or B.
* </p>
*
* @param a A Matrix. Not modified.
* @param b A Matrix. Not modified.
* @param c A Matrix where the results are stored. Modified.
*/
public static void subtract(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 = a.a11 - b.a11;
c.a12 = a.a12 - b.a12;
c.a13 = a.a13 - b.a13;
c.a21 = a.a21 - b.a21;
c.a22 = a.a22 - b.a22;
c.a23 = a.a23 - b.a23;
c.a31 = a.a31 - b.a31;
c.a32 = a.a32 - b.a32;
c.a33 = a.a33 - b.a33;
}
/**
* <p>Performs an element by element multiplication operation:<br>
* <br>
* c<sub>ij</sub> = a<sub>ij</sub> * b<sub>ij</sub> <br>
* </p>
* @param a The left matrix in the multiplication operation. Not modified.
* @param b The right matrix in the multiplication operation. Not modified.
* @param c Where the results of the operation are stored. Modified.
*/
public static void elementMult(FMatrix3x3 a, FMatrix3x3 b, FMatrix3x3 c)
{
c.a11 = a.a11 * b.a11;
c.a12 = a.a12 * b.a12;
c.a13 = a.a13 * b.a13;
c.a21 = a.a21 * b.a21;
c.a22 = a.a22 * b.a22;
c.a23 = a.a23 * b.a23;
c.a31 = a.a31 * b.a31;
c.a32 = a.a32 * b.a32;
c.a33 = a.a33 * b.a33;
}
public static void convert(DMatrix3x3 src, FMatrix3x3 dst)
{
dst.a11 = (float)src.a11;
dst.a12 = (float)src.a12;
dst.a13 = (float)src.a13;
dst.a21 = (float)src.a21;
dst.a22 = (float)src.a22;
dst.a23 = (float)src.a23;
dst.a31 = (float)src.a31;
dst.a32 = (float)src.a32;
dst.a33 = (float)src.a33;
}
public static void convert(FMatrix3x3 src, DMatrix3x3 dst)
{
dst.a11 = src.a11;
dst.a12 = src.a12;
dst.a13 = src.a13;
dst.a21 = src.a21;
dst.a22 = src.a22;
dst.a23 = src.a23;
dst.a31 = src.a31;
dst.a32 = src.a32;
dst.a33 = src.a33;
}
/**
* <p>Performs an element by element multiplication operation:<br>
* <br>
* a<sub>ij</sub> = a<sub>ij</sub> * b<sub>ij</sub> <br>
* </p>
* @param a The left matrix in the multiplication operation. Modified.
* @param b The right matrix in the multiplication operation. Not modified.
*/
public static void elementMult(FMatrix3x3 a, FMatrix3x3 b)
{
a.a11 *= b.a11;
a.a12 *= b.a12;
a.a13 *= b.a13;
a.a21 *= b.a21;
a.a22 *= b.a22;
a.a23 *= b.a23;
a.a31 *= b.a31;
a.a32 *= b.a32;
a.a33 *= b.a33;
}
/**
* Performs an in-place transpose. This algorithm is only efficient for square
* matrices.
*
* @param m The matrix that is to be transposed. Modified.
*/
public static void transpose(FMatrix3x3 m)
{
float tmp;
tmp = m.a12;
m.a12 = m.a21;
m.a21 = tmp;
tmp = m.a13;
m.a13 = m.a31;
m.a31 = tmp;
tmp = m.a23;
m.a23 = m.a32;
m.a32 = tmp;
}
public static bool hasUncountable(FMatrix3x3 a)
{
if (UtilEjml.isUncountable(a.a11 + a.a12 + a.a13))
{
return(true);
}
if (UtilEjml.isUncountable(a.a21 + a.a22 + a.a23))
{
return(true);
}
if (UtilEjml.isUncountable(a.a31 + a.a32 + a.a33))
{
return(true);
}
return(false);
}
/**
* <p>
* Returns the absolute value of the element in the matrix that has the largest absolute value.<br>
* <br>
* Max{ |a<sub>ij</sub>| } for all i and j<br>
* </p>
*
* @param a A matrix. Not modified.
* @return The max abs element value of the matrix.
*/
public static float elementMaxAbs(FMatrix3x3 a)
{
float max = Math.Abs(a.a11);
float tmp = Math.Abs(a.a12);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a13);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a21);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a22);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a23);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a31);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a32);
if (tmp > max)
{
max = tmp;
}
tmp = Math.Abs(a.a33);
if (tmp > max)
{
max = tmp;
}
return(max);
}
/**
* <p>
* Returns the absolute value of the element in the matrix that has the smallest absolute value.<br>
* <br>
* Min{ |a<sub>ij</sub>| } for all i and j<br>
* </p>
*
* @param a A matrix. Not modified.
* @return The max element value of the matrix.
*/
public static float elementMinAbs(FMatrix3x3 a)
{
float min = Math.Abs(a.a11);
float tmp = Math.Abs(a.a12);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a13);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a21);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a22);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a23);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a31);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a32);
if (tmp < min)
{
min = tmp;
}
tmp = Math.Abs(a.a33);
if (tmp < min)
{
min = tmp;
}
return(min);
}
/**
* <p>
* Transposes matrix 'a' and stores the results in 'b':<br>
* <br>
* b<sub>ij</sub> = a<sub>ji</sub><br>
* where 'b' is the transpose of 'a'.
* </p>
*
* @param input The original matrix. Not modified.
* @param output Where the transpose is stored. If null a new matrix is created. Modified.
* @return The transposed matrix.
*/
public static FMatrix3x3 transpose(FMatrix3x3 input, FMatrix3x3 output)
{
if (input == null)
{
input = new FMatrix3x3();
}
output.a11 = input.a11;
output.a12 = input.a21;
output.a13 = input.a31;
output.a21 = input.a12;
output.a22 = input.a22;
output.a23 = input.a32;
output.a31 = input.a13;
output.a32 = input.a23;
output.a33 = input.a33;
return(output);
}
