public static bool hasUncountable(FMatrix6 a)
{
if (UtilEjml.isUncountable(a.a1))
{
return(true);
}
if (UtilEjml.isUncountable(a.a2))
{
return(true);
}
if (UtilEjml.isUncountable(a.a3))
{
return(true);
}
if (UtilEjml.isUncountable(a.a4))
{
return(true);
}
if (UtilEjml.isUncountable(a.a5))
{
return(true);
}
if (UtilEjml.isUncountable(a.a6))
{
return(true);
}
return(false);
}
public static bool isIdentical(FMatrix6 a, FMatrix6 b, float tol)
{
if (!MatrixFeatures_FDRM.isIdentical(a.a1, b.a1, tol))
{
return(false);
}
if (!MatrixFeatures_FDRM.isIdentical(a.a2, b.a2, tol))
{
return(false);
}
if (!MatrixFeatures_FDRM.isIdentical(a.a3, b.a3, tol))
{
return(false);
}
if (!MatrixFeatures_FDRM.isIdentical(a.a4, b.a4, tol))
{
return(false);
}
if (!MatrixFeatures_FDRM.isIdentical(a.a5, b.a5, tol))
{
return(false);
}
if (!MatrixFeatures_FDRM.isIdentical(a.a6, b.a6, tol))
{
return(false);
}
return(true);
}
/**
* Converts {@link FMatrixRMaj} into {@link FMatrix6}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static FMatrix6 convert(FMatrixRMaj input, FMatrix6 output)
{
if (output == null)
{
output = new FMatrix6();
}
if (input.getNumRows() != 1 && input.getNumCols() != 1)
{
throw new ArgumentException("One row or column must have a length of 1 for it to be a vector");
}
int length = Math.Max(input.getNumRows(), input.getNumCols());
if (length != 6)
{
throw new ArgumentException("Length of input vector is not 6. It is " + length);
}
output.a1 = input.data[0];
output.a2 = input.data[1];
output.a3 = input.data[2];
output.a4 = input.data[3];
output.a5 = input.data[4];
output.a6 = input.data[5];
return(output);
}
/**
* Converts {@link FMatrix6} 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(FMatrix6 input, FMatrixRMaj output)
{
if (output == null)
{
output = new FMatrixRMaj(6, 1);
}
if (output.getNumRows() != 1 && output.getNumCols() != 1)
{
throw new ArgumentException("One row or column must have a length of 1 for it to be a vector");
}
int length = Math.Max(output.getNumRows(), output.getNumCols());
if (length != 6)
{
throw new ArgumentException("Length of input vector is not 6. It is " + length);
}
output.data[0] = input.a1;
output.data[1] = input.a2;
output.data[2] = input.a3;
output.data[3] = input.a4;
output.data[4] = input.a5;
output.data[5] = input.a6;
return(output);
}
public static float normF(FMatrix6 M)
{
float scale = CommonOps_FDF6.elementMaxAbs(M);
if (scale == 0.0f)
{
return(0.0f);
}
float a1 = M.a1 / scale,
a2 = M.a2 / scale,
a3 = M.a3 / scale,
a4 = M.a4 / scale,
a5 = M.a5 / scale,
a6 = M.a6 / scale;
float sum = a1 * a1 + a2 * a2 + a3 * a3 + a4 * a4 + a5 * a5 + a6 * a6;
return(scale * (float)Math.Sqrt(sum));
}
public static float fastNormF(FMatrix6 M)
{
float sum = M.a1 * M.a1 + M.a2 * M.a2 + M.a3 * M.a3 + M.a4 * M.a4 + M.a5 * M.a5 + M.a6 * M.a6;
return((float)Math.Sqrt(sum));
}
public static void normalizeF(FMatrix6 M)
{
float val = normF(M);
CommonOps_FDF6.divide(M, val);
}
