/** * Converts {@link FMatrix6x6} 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(FMatrix6x6 input, FMatrixRMaj output) { if (output == null) { output = new FMatrixRMaj(6, 6); } 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.a14; output.data[4] = input.a15; output.data[5] = input.a16; output.data[6] = input.a21; output.data[7] = input.a22; output.data[8] = input.a23; output.data[9] = input.a24; output.data[10] = input.a25; output.data[11] = input.a26; output.data[12] = input.a31; output.data[13] = input.a32; output.data[14] = input.a33; output.data[15] = input.a34; output.data[16] = input.a35; output.data[17] = input.a36; output.data[18] = input.a41; output.data[19] = input.a42; output.data[20] = input.a43; output.data[21] = input.a44; output.data[22] = input.a45; output.data[23] = input.a46; output.data[24] = input.a51; output.data[25] = input.a52; output.data[26] = input.a53; output.data[27] = input.a54; output.data[28] = input.a55; output.data[29] = input.a56; output.data[30] = input.a61; output.data[31] = input.a62; output.data[32] = input.a63; output.data[33] = input.a64; output.data[34] = input.a65; output.data[35] = input.a66; return(output); }