/** * Returns the value of the specified matrix element. Performs a bounds check to make sure * the requested element is part of the matrix. * * @param row The row of the element. * @param col The column of the element. * @return The value of the element. */ public double get( int row , int col ) { return mat.get(row,col); }
public static void setOneBasedArrayFromMatrix(double[] oneBasedArrayToPack, DenseMatrix64F matrix) { int index = 1; for (int i = 0; i < matrix.getNumRows(); i++) { oneBasedArrayToPack[index++] = matrix.get(i, 0); } }
private static boolean equalsZero(DenseMatrix64F matrix, double tolerance) { for (int rowIndex = 0; rowIndex < matrix.getNumRows(); rowIndex++) { for (int colIndex = 0; colIndex < matrix.getNumCols(); colIndex++) { if (!equals(matrix.get(rowIndex, colIndex), 0.0, tolerance)) return false; } } return true; }
public static void setMatrixRowToVector(int rowIndex, DenseMatrix64F Matrix, DenseMatrix64F vector) { for (int i = 0; i < Matrix.getNumCols(); i++) { Matrix.set(rowIndex, i, vector.get(i)); } }
public static void storeInYoVariables(DenseMatrix64F m, YoDouble[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { yoM[i][j].set(m.get(i, j)); } } }
public void setJointAngles(DenseMatrix64F angles) { for (int i = 0; i < angles.getNumRows(); i++) { oneDoFJoints[i].setQ(angles.get(i, 0)); } }
/** * Extracts the linear constraint matrix for case 2 from the full 6x10 constraint matrix. */ public static void constraintMatrix6x3( DenseMatrix64F L_6x10 , DenseMatrix64F L_6x3 ) { int index = 0; for( int i = 0; i < 6; i++ ) { L_6x3.data[index++] = L_6x10.get(i,0); L_6x3.data[index++] = L_6x10.get(i,1); L_6x3.data[index++] = L_6x10.get(i,4); } }
public static void fillMapWithMatrix(HeightMapWithPoints map, DenseMatrix64F matrix, double gridSize) { for (int i = 0; i < matrix.getNumRows(); i++) { for (int j = 0; j < matrix.getNumCols(); j++) { map.addPoint(i*gridSize, j*gridSize, matrix.get(i, j)); } } }
public static void storeInYoVariables(DenseMatrix64F v, YoDouble[] yoV) { for (int i = 0; i < v.getNumRows(); i++) { yoV[i].set(v.get(i)); } }
/** * Extracts the linear constraint matrix for case 1 from the full 6x10 constraint matrix. */ public static void constraintMatrix3x3a( DenseMatrix64F L_3x6 , DenseMatrix64F L_3x3 ) { int index = 0; for( int i = 0; i < 3; i++ ) { L_3x3.data[index++] = L_3x6.get(i,0); L_3x3.data[index++] = L_3x6.get(i,1); L_3x3.data[index++] = L_3x6.get(i,2); } }
public static void storeInYoVariables(DenseMatrix64F m, DoubleYoVariable[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { yoM[i][j].set(m.get(i, j)); } } }
public static void storeInYoVariablesVector(DenseMatrix64F v, List<YoDouble> yoV) { for (int i = 0; i < v.getNumRows(); i++) { yoV.get(i).set(v.get(i)); } }
public static void extractYoFrameQuaternionFromEJMLVector(YoFrameQuaternion yoFrameQuaternion, DenseMatrix64F matrix, int rowStart) { int index = rowStart; double x = matrix.get(index++, 0); double y = matrix.get(index++, 0); double z = matrix.get(index++, 0); double w = matrix.get(index++, 0); yoFrameQuaternion.set(x, y, z, w); }
public static void denseMatrixToMatrix3d(DenseMatrix64F denseMatrix, Matrix3d matrixToPack, int startRow, int startColumn) { for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { matrixToPack.setElement(row, column, denseMatrix.get(row + startRow, column + startColumn)); } } }
public static void denseMatrixToVector3d(DenseMatrix64F denseMatrix, Tuple3d vectorToPack, int startRow, int startColumn) { vectorToPack.setX(denseMatrix.get(startRow + 0, startColumn)); vectorToPack.setY(denseMatrix.get(startRow + 1, startColumn)); vectorToPack.setZ(denseMatrix.get(startRow + 2, startColumn)); }
public static void extractFixedFrameTupleFromEJMLVector(FixedFrameTuple3DBasics yoFrameTuple, DenseMatrix64F ejmlVector, int startIndex) { yoFrameTuple.setX(ejmlVector.get(startIndex + 0, 0)); yoFrameTuple.setY(ejmlVector.get(startIndex + 1, 0)); yoFrameTuple.setZ(ejmlVector.get(startIndex + 2, 0)); }